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ABSTRACT 

A  problem  arises  when  conventional  kinematic  equations 
that  minimize  computational  time  are  used  to  model  a  rigid 
revolute  robot  arm.  Mathematical  singularities  result  when 
successive  link  axes  "line  up"  such  that  their  angles  are  0 
or  180  degrees.  This  may  result  in  erratic  and 
uncontrollable  motion  of  the  arm  until  it  moves  away  from 
the  point  of  singularity.  One  solution  is  to  spend  a 
minimum  amount  of  time  at  the  singular  position  or  to  avoid 
it  altogether.  Another  solution  is  to  use  other  sets  of 
equations,  instead  of  the  regular  resolved-rate  equations, 
to  model  the  robot  arm.  This  thesis  shows  how  using 
equations  based  on  Newton's  Second  Principle  of  dynamics 
for  a  three  link,  two  degree  of  freedom  manipulator,  the 
problem  of  singularity  is  avoided.  The  equations  are 
demonstrated  in  a  simulation  program. 
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TABLE  OF  SYMBOLS  AND  ABBREVIATIONS 


COMPUTER        TEXT  DESCRIPTION 

SYMBOL         VARIABLE 


A  A  Sine  wave  input  torque  data 

amplitude 

AA  Aa  Acceleration  of  point  a 

AB  Ab  Acceleration  of  point  b 

AG1  Agl  The  acceleration  vector  of 

"^  the   center  of  gravity  for 

link  1 

AG2  Ag2  Same  as  Agl  but  for  link  2 

AG3  Ag3  Same  as  Agl  but  for  link  3 

AOX  aoz  Linear  acceleration  of  link 

zero  in  the  z  direction 

AOY  aoy  Linear  acceleration  of  link 

zero  in  the  y  direction 

AOZ  aoz  Linear  acceleration  of  link 

zero  in  the  z  direction 

AX1  azl  Linear  acceleration  of  link 

1  in  the  z  direction 

AY1  ayl  Linear  acceleration  of  link 

1  in  the  y  direction 

AZ1  azl  Linear  acceleration  of  link 

1  in  the  z  direction 


COMPUTER 
SYMBOL 

AX2 


AY2 


AZ2 


AX  3 


CTHETYO) 
CTHETZ ( 3 ) 
DEGRA 
ETHETXO) 

ETHETYO) 
ETHETZO) 


TEXT 
VARIABLE 

ax  2 


ay2 


az2 


ax  3 


AY3 

ay  3 

AZ3 

az3 

CTHETXO) 

c6x 

cey 


cdz 


ex 


ey 


6z 


DESCRIPTION 


Linear  acceleration  of  link 
2  in  the  x  direction 

Linear  acceleration  of  link 
2  in  the  y  direction 

Linear  acceleration  of  link 

2  in  the  z  direction 

Linear  acceleration  of  link 

3  in  the  x  direction 

Linear  acceleration  of  link 
3  in  the  y  direction 

Linear  acceleration  of  link 
3  in  the  z  direction 

A  1x3  vector  Cartesian 
value  of  the  angle  theta 
for  link  1-3  in  the  x 
direction,  results  from 
taking  the  integral  of 
angular  acceleration  in  the 
x  direction  twice,  in 
degrees 

Sane  as  c6x  but  in  the  y 
direction 

Sane  as  c6z  but  in  the  z 
direction 

Conversion  from  degrees  to 
radians 

A  1x3  vector  of  euler 
angles  for  link  1-3  in  thex 
direction,  in  degrees 

Sane  as  dx  but  in  the  y 
direction 

Sane  as  dx  but  in  the  z 
direction 


COMPUTER 
SYMBOL 

EULORYO) 


ERROR(3) 

ERRT1X 

ERRT2X 

FXO 

FYO 

FZO 

FX1 

FY1 

FZ1 

FX2 

FY2 

FZ2 


TEXT 
VARIABLE 

they  3 


Error(3) 

Errtlx 

Errt2x 

Fzo 

Fyo 

Fzo 

Fxl 

Fyl 

Fzl 

Fx2 

Fy2 

Fz2 


DESCRIPTION 


Theoretical  Euler  angle  for 
link.  3  in  the  y  direction, 
in  degrees 

%  error  between  thdy3  and 
6y  for  the  third  link  in 
the  y  direction 

%  error  between  computed 
and  input  value  of  torque 
at  joint  1 

Same  as  Errtlx  but  at  joint 
2 

Computed  force  in  the  x 
direction  at  joint  0 

Computed  force  in  the  y 
direction  at  joint  0 

Computed  force  in  the  z 
direction  at  joint  0 

Computed  force  in  the  x 
direction  at  joint  1 

Computed  force  in  the  y 
direction  at  joint  1 

Computed  force  in  the  z 
direction  at  joint  1 

Computed  force  in  the  x 
direction  at  joint  2 

Computed  force  in  the  y 
direction  at  joint  2 

Computed  force  in  the  z 
direction  at  joint  2 


Gravitational  constant 


COMPUTER 
SYMBOL 

HDX(2) 


HDY(2) 

HD2(2) 

I 

IA 

IER 


IDGT 


IXX(3,2) 


IYY(3,2) 
IZZ<3,2) 
IXZ<3,2) 


IXY<3,2) 
IYZ(3,2) 


TEXT 
VARIABLE 

HDx 


HDY 


HDz 


Ixx 


Iyy 


Izz 


Ixz 


Ixy 
Iyz 


DESCRIPTION 


The  time  rate  of  change  of 
angular  momentum  of  a  2 
element  composite  body  in 
the  x  direction 

Same  as  HDx  but  in  the  y 
direction 

Same  as  HDx  but  in  the  z 
direction 

Counter 

Row  dimension  of  matrix  A 
and  matrix  B  used  in  LEQT2F 
subroutine 

Error  parameter  used  in 
subroutine  LEQT2F 

Accuracy  test  used  in 
subroutine  LEQT2F,  for 
iterative  improvement 

A  3x2  matrix  of  Moment  of 
Inertia  for  the  two  element 
composite  body  of  link  1-3 
about  the  x  axis 

Same  as  Ixx  but  about  the  y 
axis 

Same  as  Ixx  but  about  the  z 
axis 

A  3x2  matrix  of  Products  of 
Inertia  for  the  two  element 
composite  body  of  link  1-3 
about  the  xz  coordinate 
axes 

Same  as  Ixz  but  for  the  xy 
axes 

Same  as  Ixz  but  for  the  yz 
axes 
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COMPUTER 

SYMBOL 

IXXA(3,2) 


JXO 


JYO 


JZO 


JX1 


JY1 


JZ1 


JX2 


JY2 


JZ2 


L(3,2) 


LCOGXO) 

LCOGYO) 
LCOGZ ( 3 ) 


TEXT 
VARIABLE 

Ixza 


DXO 

jyo 
jzo 
jxl 
jyi 
jzl 
jx2 
jy2 
jz2 
L(3,2) 


LCOGx 

LCOGy 
LCOGz 


DESCRIPTION 


Theoretical  Moment  of 
inertia  for  link  3  about 
joint  2 

Location  of  joint  0  in  the 
x  direction 

Location  of  joint  0  in  the 
y  direction 

Location  of  joint  0  in  the 
z  direction 

Location  of  joint  1  in  the 
x  direction 

Location  of  joint  1  in  the 
y  direction 

Location  of  joint  1  in  the 
z  direction 

Location  of  joint  2  in  the 
x  direction 

Location  of  joint  2  in  the 
y  direction 

Location  of  joint  2  in  the 
z  direction 

A  3x2  matrix  that  is  the 
distance  from  center  of 
link,  to  center  of  mass  at 
each  link  end 

A  1x3  location  of  center  of 
gravity  vector  for  link  1-3 
in  the  x  direction 

Same  as  LCOGx  but  for  the  y 
direction 

Same  as  LCOGx  but  for  the  z 
direction 
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COMPUTER 

SYMBOL 


TEXT 
VARIABLE 


DESCRIPTION 


Used  in  LEQT2F  subroutine 
as  number  of  right  hand 
sides 


MASS(3,2) 


Mass(3,2) 


A  3x2  matrix  of  mass  of 
each  element  that  make  up 
the  composite  body  for  link 
1-3 


MASS1 

Ml 

MASS2 

M2 

MASS3 

M3 

MATA(27, 

27) 

MatA 

MATB(27) 


MatB 


MI 


MJ 


MK 


MIAO,  MJAO  and 
MKAO 


MI BO,  MJBO  and 
MKBO 


Total  mass  of  link  1 

Total  mass  of  link  2 

Total  mass  of  link  3 

A  27x27  matrix  consisting 
of  coefficients  of  the 
unknown  variables 

A  1x27  vector  consisting  of 
the  coefficient  of  known 
variables  on  input  to 
subroutine  LEQT2F  and  an 
output  the  solution  to  the 
linear  equations 

Results  from  subroutine 
CPROD,  i  component  of 
vector  cross  product 

j  component  of  vector 
cross  product 

k  component  of  vector 
cross  product 

Cross  product  between  wdl 
and    RB/G1    at   joint   0, 
link   1,   in   the  x,      y,  z 
direction 

Cross  product  between  wl 
and  RB/G1  at  joint  0,  link 
1,  in  the  x,  y,  z  direction 
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COMPUTER 
SYMBOL 

MI CO,  MJCO  and 
MKCO 


MIA1,  MJA1  and 
MKA1 


MIB1,  MJB1  and 
MKB2 


MIC1,  MJC1  and 
MIC1 


TEXT 
VARIABLE 


MIA2,  MJA2  and 
MKA2 


DESCRIPTION 


Cross  product  between  v»l 
and  MIBO,  MJBO  and  MKBO  at 
joint  0,  link  1,  in  the  x, 
y,  z  direction 

Cross  product  between  wdl 
and  RA/G1  at  joint  1,  link 
1,  in  the  x,  y,  z  direction 

Cross  product  between  wl 
and  RA/G1   at  joint  1,  link 

1,  in  the  x,  y,  z  direction 

Cross  product  between  wl 
and  MIB1,  MJB1  and  MKB1 
respectively  at  joint  1, 
link  1,  in  the  x,  y,  z 
direction 

Cross  product  between  wd2 
and  RB/G2   at  joint  1,  link 

2,  in  the  x,  y,  z  direction 


MIB2,  MJB2  and 
MKB2 


MIC2,  MJC2  and 
MKC2 


Cross  product  between  w2 
and  RB/G2  at  joint  1,  link 
2,  in  the  x,  y,  z  direction 

Cross  product  between  w2 
and  HIB2,  MJB2  and  MKB2 
respectively  at  joint  1, 
link  2,  in  the  x,  y,  z 
direction 


MIA3,  MJA3  and 
MJA3 


MIB3,  MJB3  and 
MKB3 


Cross  product  between  wd2 
and  RA/G2  at  joint  2,  link 
2,    in  the  x,  y,  z  direction 

Cross  product  between  w2 
and  RA/G2  at  joint  2,  link 
2,  in  the  x,  y,  and  z 
direction 
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COMPUTER 
SYMBOL 

MIC3,  MJC3  and 
MKC3 


TEXT 
VARIABLE 


MIA4,  MJA4  and 
MKA4 


MIB4,  MJB4  and 
MKB4 


MIC4,  MJC4  and 
MKC4 


N 


DESCRIPTION 


Cross  product  between  w2 
and   MIB3,   MJB3   and   MKB3 
respectively   at   joint  2, 
link  2,      in  the   x,   y,  z 
direction 

Cross  product  between  wd3 
and  RA/G3  at  joint  2,  link 
3,    in  the  x,  y,  z  direction 

Cross  product  between  w3 
and  RA/G3  at  joint  2,  link 
3,  in  the  x,  y,  z  direction 

MJC4,  MJC4  and  Cross 
product  between  w3  and 
MIB4,  MJB4,  and  MKB4 
respectively  at  joint  2, 
link  3,  at  the  x,  y,  z 
direction 

Used  in  LEQT2F  subroutine 
for  the  order  of  Mat A  and 
number  of  rows  of  vector  B 


RUN 


RX(3,2) 


Rx(3,2) 


RY(3,2) 

RZ(3,2) 


Ry(3,2) 

Rz(3,2) 


Phase   angle   of 
input  to  joints 

Number     of 
conducting 


sine  wave 


the 


run 


A  3x2  matrix  consisting  of 
the  distance  from  the 
center  of  gravity  of  the 
link  to  center  of  mass  for 
the  elements  of  link  1-3  in 
the  x  direction 

Same  as  Rx(3,2)  but  in  the 
y  direction 

Same  as  Rx(3,2)  but  in  the 
z  direction 
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COMPUTER 
SYMBOL 

RAGK3) 


RBGK3) 


RAG2(3) 


RBG2(3) 


RBG3(3) 


SUMHDXO) 


THETYR(3) 

THETZRO) 


TOX,    TOY 
TOZ 


TEXT 
VARIABLE 

ra/Gl 


rb/Gl 


ra/G2 


rb/G2 


rb/G3 


EHDx 


SUMHDYO) 

EHDy 

SUMHDZO) 

EHDz 

THETXR(3) 

Tox,  Toy, 
Toz 


DESCRIPTION 


A  1x3  vector,  distance  of 
point  a  to  center  of 
gravity  for  link  1,  in  the 
x,  y,  z   direction 

A  1x3  vector,  distance  of 
point  b  to  center  of 
gravity  for  link  1,  in  the 
x,  y,  z  direction 

A  1x3  vector,  distance  of 
point  a  to  CoG  for  link  2, 
in  the  x,y,z  direction 

A  1x3  vector,  distance  of 
point  b  to  CoG  for  link  2, 
in  the  x,  y,  z  direction 

A  1x3  vector,  distance  of 
point  b  to  CoG  for  link  3, 
in  the  x,  y,  z  direction 

A  1x3  vector,  sun  of  HDX 
for  the  two  elements  of 
link  1-3  in  the  x  direction 

Sane  as  EHDx  but  in  the  y 
direction 

Sane  as  EHDx  but  in  the  z 
direction 

A  1x3  vector  of  euler 
angles  in  the  x  direction 
in  radians  for  link  1-3 

Same  as  THETXRO)  but  in 
the  y  direction 

Sane  as  THETXRO)  but  in 
the  z  direction 

Input  torque  at  joint  0  at 
the  x,  y,  z  direction 
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COMPUTER 
SYMBOL 

T1X,  T1Y 
T1Z 

T2X,  T2Y 
T2Z 

THDDOTO) 


THEORY ( 3 ) 


TEXT 
VARIABLE 

Tlx,  Tly 
Tlz 

T2x,  T2y 
T2z 


DESCRIPTION 


Input  torque   at  joint  1  at 
the  x,  y,  z  direction 

Input  torque  at  joint   2  at 
the  z,  y,  z  direction 

Theoretical   value   of   wdx 
for  link  3  in  degrees 

Theoretical   value   of   wdx 
for  link  3  in  radians 


THEXR1,  THEXR2, 
THEXR3 

THEYR1,  THEYR2, 
THEYR3 


Second  integral   of  wdx  for 
links  1-3  in  radians 

Second  integral   of  wdy  for 
links  1-3  in  radians 


THEZR1,  THEZR2, 
THEZR3 

TORY IX 


T0RY2X 


TX1,  TX2,  TX3 


TY1,  TY2,  TY3 


TZ1,  TZ2,  TZ3 


VECTAOO)  and 
VECTBO ( 3 ) 


Torylx 


Tory2x 


Second  integral  of  wdz  for 
link  1-3  in  radians 

Computed  value  of  torque 
for  joint  1 

Computed  value  of  torque 
for  joint  2 

Euler  angle  theta  converted 
to  radians  for  links  1-3 
respectively  in  the  x 
direction 

Euler  angles  theta 
converted  to  radians  for 
links  1-3  respectively  in 
the  y  direction 

Euler  angles  theta 
converted  to  radians  for 
links  1-3  respectively  in 
the  z  direction 

1x3  vector  used  in 
subroutine  CPROD  for  joint 
0 
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COMPUTER 
SYMBOL 

VECTAK3)    and 
VECTBK3) 


VECTA2(3)  and 
VECTB2(3) 


VECTA(3)  and 
VECTB(3) 


TEXT 
VARIABLE 


Wl,  W2,  and 
H3 

WX(3) 


WY(3) 

HZ(3) 

W0X(3) 

HDY(3) 

H0Z(3) 

HKAREA 

XI,  X2  and 

X3 

Wl,»2,»3 
wx(3> 

wy(3) 

wz<3) 

wdx(3) 

wdy(3) 

wdz(3) 


DESCRIPTION 


1x3  vector  used  in 
subroutine  CPROD  for  joint 
1 

1x3  vector  used  in 
subroutine  CPROD  for  joint 
2 


1x3    vector    used 
subroutine  CPROD 


in 


Frequency  of   sine  function 
input 

Heights  of  link  1,  2,  and  3 


A  1x3  vector  of  angular 
velocity  of  link  1-3  in  the 
x  direction 

Same  as  wx(3)  but  in  the  y 
direction 

Sane  as  wx(3)  but  in  the  z 
direction 

Angular  acceleration  of 
link  1-3  in  the  x  direction 

Angular  acceleration  of 
link  1-3  in  the  y  direction 

Angular  acceleration  of 
link  1-3  in  the  z  direction 

Work  area  for  LEQT2F 
subroutine 

Location  of  center  of 
gravity  for  link  1-3  in  the 
x  direction 

Theoretical  value  of  y 
distance  from  Fz2  to  center 
of  gravity  of  link  3 
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COMPUTER  TEXT  DESCRIPTION 

SYMBOL  VARIABLE 

Yl,  Y2  and  Location   of    center    of 

Y3  gravity  for  link  1-3  in  the 

y  direction 

Z  z  Theoretical    value   of   z 

distance  from  Fy2  to  center 
of  gravity  of  link.  3 

Zl,  Z2  and  Location   of    center    of 

Z3  gravity  for  link  1-3  in  the 

z  direction 
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I.  INTRODUCTION 

Manipulator  models  which  use  local  coordinates  as  a 
basis  for  simulation  and  control  have  a  mathematical 
singularity  built  into  them  (Ref.  1].  This  singularity 
occurs  when  rigid  robot  links  align  such  that  their 
relative  position  is  either  0°  or  180°.  When  this  happens, 
the  inverse  of  the  Jacobian  matrix  becomes  impossible  to 
compute  and  the  forward  dynamics  solution  cannot  be  found. 
In  the  control  of  serial  link  manipulators  there  have 
been  various  approaches  which  use  local  coordinates  to 
achieve  computational  efficiency.  One  method  deals  with  the 
Newton-Euler  approach  [Refs.  2,  31,  another  uses  the 
Lagrangian  approach  [Refs.  4,  5]  or  there  is  the  method  of 
virtual  work  (Ref.  6].  Still  another  that  has  tried  to  make 
the  solution  to  the  dynamic  equations  computationally 
efficient  by  using  Kane's  Dynamical  equations  [Ref.  71. 
However,  although  these  methods  have  been  computationally 
efficient,  they  have  not  been  able  to  handle  the  problem  of 
singularity  [Ref.  1]. 
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Various  methods  have  been  proposed  to  deal  with  the 
problem  of  singularity.  One  such  method  is  to  minimize  the 
time  near  the  singularity  [Ref.  8],  thereby  reducing  its 
effects.  Another  solution  is  to  avoid  the  position  of  the 
manipulator  that  caused  the  singularity  [Refs.  9,  10]. 
However,  when  using  resolved  rate  equations  the  arm  may 
pass  through  a  point  of  singularity  anyway,  in  response  to 
a  command  [Ref.  11].  Nakamura  and  Hanafusa  [Ref.  12]  have 
proposed  to  determine  the  joint  motion  for  the  requested 
motion  of  the  end  effector  by  evaluating  the  feasibility  of 
the  joint  motion.  This  determined  joint  motion  is  called  an 
inverse  kinematic  solution  with  singularity  robustness. 
Other  solutions  deal  with  presenting  equations  that  can 
translate  the  manipulators  in  the  neighborhood  of 
singularity  [Refs. 13, 14]  and  in  identifying  geometric 
singular  positions  (Ref.  15]. 

It  has  also  been  shown  that  redundancy  of  robot 
manipulators  is  effective  in  dealing  with  singularities 
[Ref.  16].  Klein  and  Huang  [Ref.  17]  have  studied  the 
method  of  pseudo-inverse  control,  for  use  with  redundant 
manipulators,  with  recommendations  for  improvement. 
Uchiyama  [Ref.  18]  proposed  switching  the  control  mode  in 
the  neighborhood  of  singular  points  from  the  mode  using 
inverse  kinematics  to  the  joint  control  mode.  A  seven 
degree  of  freedom  kinematic  design  with  a  spherical 
shoulder  joint   was  proposed   (Refs.  19,   20],  as  well  as  a 
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seven  joint  robot  [Ref.  21]  to  handle  singularity.  A  four 
degree  of  freedom  wrist  was  studied  to  overcome  wrist 
singularity  [Ref.  22].  Shin  [Ref.  23]  looked  at  the 
physical  quantities  and  combinations  of  physical  quantities 
which  are  unaffected  by  redundancy  to  simplify  the 
solution  of  a  redundant  system.  However,  even  though  there 
are  some  redundant  manipulators  constructed  [Refs.  24,  25], 
research  cannot  do  away  with  singularities,  and  so 
consideration  still  has  to  be  given  to  the  control  of  the 
manipulator  in  case  of  inadvertant  singularities. 

This  paper  will  derive  equations  of  motion  using  the 
First  Principles  of  Newtonian  dynamics  in  terms  of  global 
coordinates  in  order  to  eliminate  the  problem  of 
singularity.  By  the  method  of  free  body  analysis,  each  link 
of  the  manipulator  is  treated  as  if  it  were  a  free  body 
with  forces  and  moments  applied  at  the  joints.  Only 
revolute  joints  will  be  considered.  Although  tedious  and 
time  consuming  (computer  time),  this  paper  will  show  by 
simulation  how  the  problem  of  singularity  may  thus  be 
overcome . 
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II.  POROT  MnpfiLMMC  AMD  SIMULATIQil  PROBLEM 

This  thesis  does  not  deal  with  the  control  aspect  of 
rigid,  revolute  linkages  but  rather  the  mathematical 
dynamic  modelling.  Given  the  dynamic  model,  the  link  masses 
and  inertia  properties,  initial  link  alignments,  and  joint 
torques,  then  the  joint  forces,  acceleration,  velocity  and 
position  can  be  predicted  via  a  simulation  program.  In  the 
present  approach,  all  dynamic  properties  except  for 
acceleration  and  forces  were  assumed  to  remain  constant 
over  a  simulated  time  interval.  This  assumption  linearized 
the  equation  of  motion  so  that  a  simple  matrix  inversion 
could  be  used  to  solve  for  the  unknowns.  As  shown  in  Figure 
1,  the  simulation  is  updated  with  the  predicted  velocity 
(£,)  and  position  (§.),  following  integration  at  the  next 
time  step.  Simulation  validation  is  done  by  comparing  the 
theoretical  position  (th6y3)  to  the  predicted  position 
(0y3)  for  link  3  and  actual  torque  (Tlx,  T2x)  to  computed 
torque  (Torylx,  Tory2x)  for  links  2  and  3. 


Pynaml c 
Model 


In  tegra  tor 


9.  e 


Fipure  1.  Manipulator  Simulation  Block  Piapram 
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III.  THEORETICAL  DEVELOPHEMT 

A.  MANIPULATOR  ARM  CONFIGURATION 

This  thesis  develops  a  generalized  simulation  program 
for  a  robot  manipulator  that  is  a  serial  connection  of 
three  rigid  links,  jointed  by  one-degree  of  freedom 
revolute  joints.  Joint  actuators  are  assumed  to  be  located 
between  successive  links  to  apply  the  torque  necessary  to 
position  the  link. 

B .  THEORY 

The  method  of  solution  is  based  on  the  principle  of 
free  body  analysis.  For  this  approach  each  body  of  the 
three  link  manipulator  is  treated  as  if  it  were  a  free  body 
with  forces  and  moments  applied  at  each  joint,  as  shown  in 
Figure  2.  The  global  cartesian  coordinate  system  X,  Y,  Z  as 
well  as  force  and  moment  torque  conventions  are  also 
evident  in  the  figure.  Note  that  a  local  coordinate  system, 
that  is  a  coordinate  system  that  is  local  to  each  joint, 
will  not  be  used  but  rather  a  single  global  system  will  be 


24 


Note:  All  links  reside 
in  the  v-z   rlane 


Joint  2 


o  1  n  t  i 


Link  1 


Figure  2.  Free  Body  Diagrams 
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adopted.  So  all  positions,  distances,  etc.,  will  be 
referenced  to  the  base  of  the  manipulator  system  which  will 
be  at  joint  zero.  The  effects  of  flexibility  of  the  robot 
manipulator  will  not  be  considered  since  ideal,  rigid 
bodies  are  assumed. 

In  developing  the  dynamic  equations  of  motion  for  each 
link  Newton's  Second  Principle  of  Motion  is  used.  The  known 
variables  are  torque  at  the  joints,  mass  of  each  link, 
linear  acceleration  of  joint  zero,  initial  angular 
acceleration,  angular  velocity  and  position  of  all  links. 
The  unknowns  are  the  forces  at  the  joints,  linear  and 
subsequent  angular  accelerations  of  the  links. 

C.   DYNAMIC  EQUATIONS  OF  MOTION  OF  LINK  ONE 
1.  Sum  of  Forces  Equations 

In  the  free  body  analysis  of  link  one  (Figure  2)  the 
sum  of  the  forces  in  the  z  direction  is: 

£Fx=Fxl  -  FxO=  Mlaxl  (1) 

Similarly  sum  of  the  forces  in  the  y  direction  is: 
EFy-Fyl  -  FyO*Mlayl  (2) 

and  the  sum  of  the  forces  in  the  z  direction  is: 
EFz=Fzl  -  FzO  -  Wl=Mlazl  (3) 
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2.  Joint  Equations 

He  begin  by  evaulating  the  joint  equations  at  joint 
zero  (Ref.  26,  equation  (8/4),  pp.  423).  If  the  joint  is 
sequested  and  analysis  conducted  at  a  point  on  link  zero 
(subscript  a)  and  another  at  a  point  on  link  one  (subscript 
b)  that  is  common  to  both,  so  when  linked  together  they  are 
equal.  This  results  in  two  equations  and  the  two  unknowns 

wdl  and  wl. 
As  a  result: 
Aa  =  Ao 
which  is  the  acceleration  at  joint  zero, 
and 

Ab  =  Al  +(wdl  X  rb/Gl)  +  wl  X  (wl  X  rb/Gl) 
which  is   the  acceleration  of  point  b  on  joint  one.  Here 
rb/Gl  is  the  distance  from  point  b  to  the  center  of  gravity 
of  link   one,  and   Al  is   the  acceleration  at  the  center  of 
mass  of  link  one  or, 

rb/Gl*(jxO-LCOGxl)i  +  ( jyO-LCOGyl) j  +  ( jzO-LCOGzl)k 
=rb/Glx  +  rb/Gly  +  rb/Glz 
After  equating  Aa  and  Ab  and  having  the   known  variables  on 
the  right  side  of  the  equation  and  unknown  variables  on  the 
left  side  the  following  sets  of  equations  result: 

Axl  +  wdyl(rb/Glz)-wdzl(rb/Gly)=  Aox-MICO         (4) 
where  MICO  equals 


27 


=wylwxl(rb/Gly)-w*yl(rb/Glx)-w*zl(rb/Glx) 
+  wzlwxl(rb/GIz) 
also 

Ayl  +wdzl(rb/Glx)-wdxl(rb/Glz)=Aoy-MJCO  (5) 

where  HJCO  equals 

=wzlwyl(rb/Glz)-wax(rb/Gly)-w3,xl(rb/Gly) 

-i-  wxlwyl(rb/Glx) 
and 

Azl  +  wdxl(rb/Gly)-wdyl(rb/Glx)=Aoz-MKCO         (6) 

MKCO  equals 

=wxlwzl(rb/Glx)-w*xl(rb/Glz)-wayl(rb/Glz) 

+  wylwzl(rb/Gly) 

3.  Sum  of  Moment  Equations 

Computing  the  sum  of  the  moment  equations  about  the 

center  of  gravity  results  in: 

EMl  =  (rOA51  X  FO)  +  (rl/Gl  X  F1)-T1  +  TO 

where  the   vector  rO/Gl   is  the  distance  from  joint  zero  to 

the  center  of  gravity  of  link  one  and  vector  rl/Gl   is  the 

distance  from  joint  one   to  the   center  of  gravity  of  link 

one,  in  the  x,  y  and  z  directions.  Such  that 

rOTGl  =  rjo-rGl 

and 

rl/Gl  -  rjl-rGl 

so 

rjO-rGl=(xjO-xGl)i  +  (yjO-yGl)j  +  (zjO-zGl)k 

and 
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ril-rGl  =  (xjl-xGl)i  +  (yjl-yGDj  +  zjl-zGDk 
In  the  x,  y  and  z   directions  the   sum  of   moment  equations 
are: 

EMI  in  z  direction- 

(-yjO/GDFzO  +  (zjO/GDFyO  +  (yj  l/Gl)Fzl-(zj  1/G1  )Fyl 
-Tlx  ♦  TOx  (7a) 

EMI  in  y  directions 

(-zjO/GDFxO  +  (xjO/GDFzO  +  (zjl/GDFxl-  (xjl/GDFzl 
-Tly  +  TOy  (8a) 

EMI  in  z  direction^ 

(-xjO/GDFyO  +  (yjO/GDFxO  •►  (xjl/Gl)Fyl-(yjl/Gl)Fxl-Tlz 
+  TOz  (9a) 

From  Ref.  26,  equation  (517)  pp.227  the  sum  of  the  moments 
about  a  fixed  point  that  does  not  move  with  the  body  is 
equal  to  the  time  rate  of  change  of  angular  momentum  of  the 
system  (H)  about  the  fixed  point,  EM=H.  In  the  present 
study  we  have  let  each  link  be  a  composite  body  of  two 
elements.  The  angular  momentum  (H)  for  a  composite  body 
where  the  number  of  elements  of  the  body  is  two,  about  the 
center  of  gravity  of  each  link  is  Hi=E*  (Ri  X  (w  X  Ri)]Mi, 
where  Ri  is  the  distance  from  the  center  of  gravity  of  each 
link  to  the  appropriate  element  (lor 2)  in  the  x,  y  and  z 
direction.  So: 

Hx  =  Ea  [Ryi(wx(Ryi)-wy(Rxi))-Rzi(wz(Rxi)wx-(Rzi))]Mi 
Hx     =[RZyl(wx)-Ryl(Rxl)(wy)-Rz(Rxl)(wz)     +    RzKwxHMl 
+[Ray2(wx)-Ry2(Rx2)(wy)-Rz2(Rx2)(wz)  +  (R*z2)wx)M2 

29 


If  Ixx  ■  fRy  +Rza  dm. 


f 


and  Ixy  =  |*RxRy  dm, 

and 

Ixz  =fRxRy  dm, 

then: 

Hx  =  [Ilxx(wx)  -  Ilxy(wy)  -  Ilxz(wz))Ml 

+  [I2xx(wx)  -  I2xy(wy)  -  I2xz(wz))M2. 
and 

HDx  -  (Ilxx(wdx)  -  Ilxy(wdy)  -  Ilxz(wdz)]Ml 

+  tI2xx(wdx)  -  I2xy(wdy)  -  I2xz(wdz)JM2     (7b) 
by  assuming  the  moment  of  inertia  does  not  change  with  time 
but  is  constant  for  a  given  time  interval. 

By  similar  analysis  it  can  be  shown: 

Hy=  z      CRzi(wy(Rzi)-wz(Ryi))-Rxi(wx(Ryi)-wy(Rxi)) ]Mi 
ana  W  ^  jrf  +  rf  a., 
and     Iyz=  fRyRz  dm, 
and     Ixy=  fRxRy  dm 

then: 

HDy=[ I lyy ( wdy ) -I iyz ( wdz ) -I iyx ( wdx ) ) Mi 

+  CI2yy(wdy)-I2yz(wdz)-I2yx(wdx) )M2  (8b) 

and 

Hz=E*  [Rxi(wz(Rxi)-wx(Rzi))-Ryi(wy(Rzi)-wz(Ryi))}Hi. 
If   Izz=  fRx  -t-Ry   dm. 


P 


So 
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Hz=[Ilzz(wz)-Ilyz(wy)-Ilzx(wx)]Ml 

+  [I2zz(wz)-I2yz(wy)-I2zx(wx)]M2 
then 
HDz=[Ilzz(wdz)-Ilyz(wdy)-Ilzx(wdx)]Ml 

+  (I2zz(wdz)-I2yz(wdy)-I2zx(wdx) JM2  (9b) 

Combining  equations   (7a)   and   (7b)   and  keeping  known 
variables  on  the  right  side  and  unknown  variables  on  the 
left  side  yields: 
EMlx=  (-yjo/GDFzo  +  (zjo/GDFyo  +  (yjl/GDFzl 

-  (zjl/Gl)Fyl-HDx=Tlx-Tox  (7) 

Combining  equations  (8a)  and  (8b)  yields: 
ZMly=(-zjo/Gl)Fxo  +  (xjo/GDFzo  +  (zjl/GDFxl 

-  (xjl/Gl)Fzl-HDy=Tly-Toy  (8) 
combining  equations  (9a)  and  (9b)  yields: 
EMlz=-(xjo/Gl)Fyo  +  (yjo/GDFxo  +  (xjl/GDFyl 

-  (yjl/Gl)Fxl-HDz*Tlz-Toz  (9) 

D.   LINK  TWO  EQUATIONS 

1.  Sum  of  Forces  Equations 

From   the   free  body  diagram  (Figure  2)  it  follows 
that 

EFx«Fx2  -  Fxl=M2ax2  (10) 

EFy»Fy2  -  Fyl=M2ay2  (11) 

EFz=Fx2  -  Fzl=M2az2  (12) 
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2.  Joint  Equations 

Analysis  is  conducted  at  joint  one  where  similar 
equations  are  used  as  in  joint  zero  with  a  point  on  link 
one  (a)  and  one  on  link  two  (b).  For  point  a  the  equation 
is 

Aa=  A1  +  wdl  X  ra/Gl  +  wl  X  (wl  X  ra/Gl) 
ra/Gl  is  a  vector  whose  distance   is  measured   from  point  a 
to  the   center  of   gravity  of   link  one   in  the   x,  y  and  z 
direction. 

ra/Gl=(jxl-LCOGxl)i  +  ( jyl-LCOGyl ) j  +  ( jzl-LCOGzl)k 
=ra/Glx  +  ra/Gly  +  ra/Glz 
For  point  b  the  equation  is: 

Ab=A  2  *  wd2  X  rb/G2  +  w2  X  (w2  X  rb/G2) 

**  «"\»  A*  >**  i*+  s^  *^ 

where  rb/G2  is  a  vector   whose   distance   is   measured  from 
point  b  to  the  center  of  gravity  of  link  two. 

rb/G2=( jxl-LC0Gx2)i  +  ( jyl-LC0Gy2) j  +  ( jzl-LC0Gz2)k 

=rb/G2x  +  rb/G2y  +  rb/G2z 

Equating  Aa  and  Ab  and  setting  knowns  and  unknowns  on 
the  respective  sides  of  the  equation  results  in: 

Ax2-Axl  +  wdy2(rb/G2z)-wdz2(rb/G2y)-wdyl(ra/Glz)  + 

wdzl(ra/Gly)=MICl-MIC2  (13) 

MICl»wylwxl( ra/Gly )-w2yl(ra/Glx)-w2zl(ra/Glx) 
♦  wzlwxl(ra/Glz) 

MIC2=wy2wx2(ra/G2y)-w2y2(ra/G2x)-w2z2(rb/G2x) 

-t-   wz2wx2(rb/G2z) 
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Ay2-Ayl  +  wdz2(rb/G2x)-wdx2(rb/G2z)-wdzl(ra/Glx) 
♦  wdxl(ra/Glz)=MJCl-MJC2  (14) 

MJCl=wzlwyl(ra/Glz)-w2zl(ra/Gly)-w2xl(ra/Gly) 
+  wxlwyl(ra/Glx) 

MJC2=wz2wy2(rb/G2z)-w2z2(rb/G2y)-w2x2(rb/G2y) 

+  wx2wy2(rb/G2x) 
AZ2-AZ1  +  wdx2(rb/G2y)-wdy2(rb/G2x)-wdxl(ra/Gly) 

+  wdyl(ra/Glx)=  MKC1-MKC2  (15) 

HKCl=wxlwzl(ra/Glx)-w2xl(ra/Glz)-w2yl(ra/Glz) 

+  wylwzl(ra/Gly) 
HKC2=wx2wz2(rb/G2x)-w2x2(rb/G2z)-w2y2(rb/G2z) 

+  wy2wz2(rb/G2y) 

3.  Sum  of  the  Moment  Equations 

These  equations  have  a  similar  development  as  that 
of  link  one: 

EM2  =(rjl/G2)  X  Fl  +  (rj2/G2)  X  F2  +  T1-T2 
where 

rjl/G2=(xjl-xG2)i  +  (yjl-yG2)j  +  (zjl-zG2)k 

rj2/G2=(xj2-xG2)i  +  (yj2-yG2)j  +  (zj2-zG2)k 
ZM2x=  -(yjl-yG2)Fzl  +  (zjl-zG2)Fyl  +  (yj2-yG2)Fz2 

-  (zj2-zG2)Fy2  +   Tlx-T2x  (16a) 
EM2y=  -<zjl-zG2)Fxl  +  (xjl-xG2)Fzl  +  (zj2-zG2)Fx2 

-  (xj2-xG2)Fz2  +  Tly-T2y  (17a) 
EM2z=-  (xjl-xG2)Fyl  +  (yjl-yG2)Fxl  +  (xj2-xG2)Fy2 

-  (yj2-yG2)Fx2  +  Tlz-T2z  (18a) 
From  angular   momentum  equation  developed  for  link  one,  it 
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can  be  shown  for  link  two: 

EH2x=HDx  (16b) 

EM2y=HDy  (17b) 

EM2z=HDz  (18b) 

Combining  equations  (16a)  and  (16b)  the  following  result: 
-(yjl-yG2)Fzl  +  (zjl-zG2)Fyl  +  (yj 2-yG2)Fz2-(zj2-zG2)Fy2 
-HDx»-Tlx  +  T2x  (16) 

Combining  equations  (17a)   and   (17b)   yield   the  following 
result: 
-(zjl-zG2)Fxl  ♦  (xjl-xG2)Fzl  +  (zj2-zG2)Fx2-(x}2-xG2)Fz2 

-HDy=-Tly  +  T2y  (17) 

Combining  equations   (18a)   and   (18b)  yield  the  following 
result: 
-(xjl-xG2)Fyl  +  (yjl-yG2)Fxl  +  (xj2-xG2)Fy2-(yj2-yG2)Fx2 
-HDz=-Tlz  +  T2z  (18) 

E.   LINK  THREE  EQUATIONS 

1.  Sum  of  Forces  Equations 

Following   similar  logic    from    that   previously 
developed: 

EFx=  -Fx2=H3ax3  (19) 

£Fy«  -Fy2=M3ay3  (20) 

ZFz=  -Fz2  -  W3=M3az3  (21) 

2.  Joint  Equations 

With  point  a  on  link  two  and  point  b  on  link  three 
one  gets  for  joint  equations  at  joint  two: 

34 


Aa=A2  +  (wd2  X  ra/G2)  +  w2  X  <w2  X  ra/G2) 
where   ra/G2   is   a  vector  whose  distance  is  measured  from 
point  a  to  center  of  gravity  of  link  two  in  the  x,y  and  z 
direction. 
ra/G2=( jx2-LCOGx2)i  +  ( jy2-LCOGy2) j  +  ( jz2-LCOGz2)k 

=ra/G2x  ¥   ra/G2y  ♦  ra/G2z 
For  point  b 

Ab=A3  -i-  wd3  X  rb/G3  +  w3  X  (w3  X  rb/G3) 
where   rb/G3   is  a  vector  whose  distance  is  measured  from 
point  b  to  center  of  gravity  of  link  three  in  the  x,   y  and 
z  direction. 
rb/G3=( jx2-LCOGx3)i  +  ( jy2-LCOGy3) j  +  ( jz2-LCOGz3)k 

=rb/G3x  f  rb/G3y  +  rb/G3z 
Equating  Aa   and  Ab  and  setting  knowns  and  unknowns  on  the 
respective  sides  of  the  equation  results  in: 

Ax3-Ax2  +  wdy3(rb/G3z)-wdz3(rb/G3y)-wdy2(ra/G2z) 

+  wdz2(ra/G2y)=MIC3-MICl  (22) 

MIC3=wy2wx2(ra/G2y)-w2y2(ra/G2x)-w2z2(ra/G2x) 

f  wz2wx2(ra/G2z) 
MIC4=  wy3wx3(rb/G3y)-w2y3(rb/G3x)-w2z3(rb/G3x) 

♦  wz3wx3(rb/G3z) 
Ay3  -Ay2  ♦  wdz3(rb/G3x)-wdx3(rb/G3z)-wdz2(ra/G2x) 

+wdx2 ( ra/G2z ) =MJC3-MJC4  (  23 ) 

MJC3=wz2wy2(ra/G2z)-w2z2(ra/G2y)-w2x2(ra/G2y) 
♦  wx2wy2(ra/G2x) 
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MJC4=wz3wy3(rb/G3z)-w2z3(rb/G3y)-w2x3(rb/G3y) 

+  wx3wy3(rb/G3x) 
AZ3-AZ2  +  wdx3(rb/G3y)-wdy3(rb/G3x)-wdx2(ra/G2y) 

+  wdy2(ra/G2x)=  MKC3-MKC4  (24) 

MKC3=wx2wz2(ra/G2x)-w2x2(ra/G2z)-w2y2(ra/G2z) 

+  wy2wy2(ra/G2y) 
MKC4=wx3wz3(rb/G3x)-w2x3(rb/G3z)-w2y3(rb/G3z) 
+  wy3wz3(rb/G3y) 
3 ♦Sam  of  Moment  Equations 

As  in  the  development  of  the  equations  for  link,  one: 
EM3=(rj2/G3)  X  F2  +  T2 
where  r j2/G3«(xj2-xG3) i  +  (yj2-yG3)j  +  (zj2-zG3)k 

=xj2/G3  +  yj2/G3  +  ZJ2/G3 
EM3x*(-yj2/G3)Fz2  +  (zj2/G3)Fy2  +  T2x  (25a) 

ZM3y=   (-zj2/G3)Fx2  +  (xj2/G3)Fz2  ♦  T2y  (26a) 

EM3z=(-xj2/G3)Fy2  +  (yj2/G3)Fx2  +  T2z  (27a) 

From  the  angular  momentum  theory: 

£M3x=HDx  (25b) 

£M3y»H0y  (26b) 

EM3z=HDz  (27b) 

Combining  equations   (25a)  and  (25b)  the  following  results: 

(-yj2/G3)Fz2  +  (zj2/G3)Fy2-HDx=  -T2x  (25) 

Combining  equations  (26a)  and  (26b)  the   following  results: 
(    (-zj2/G3)Fx2  +(xj2/G3)Fz2-HDy=  -T2y  (26) 

Combining  equations  (27a)  and  (27b)  the  following  results: 
(-xj2/G3)Fy2  +  (yj 2/G3)Fx2-HDz=  -T2z  (27) 
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IV.  COMPUTATIONAL  APPROACH 

The  language  chosen  to  write  the  program  was  the 
Digital  Simulation  Language  (DSL)  using  Fortran  77  coding. 
This  language  does  an  excellent  dynamic  simulation  that 
allows  the  user  to  be  interactive,  with  real  time 
processing  vice  batch  mode  processing  commonly  used  with 
the  Continuous  System  Modelling  Program  (CSMP),  and  all 
calculations  done  in  double  precision.  The  source  code 
produced  for  this  program  was  complied  on  an  IBM  3033 
computer  using  a  Fortran  77  compiler. 

A.   PRINCIPLE  PROGRAM  MATRICIES 

A  27x27  Matrix  A  (MatA)  was  created  from  the 
coefficients  of  the  unknowns  (forces,  linear  acceleration 
and  angular  acceleration)  from  equations  (1)  to  (27). 
Correspondingly  a  27x1  Matrix  B  (MatB)  was  generated  from 
equations  (1)  to  (27)  from  all  knowns  (torques,  angular 
velocities,  link  masses,  and  various  positions).  Subroutine 
CPROD  was  used  to  conduct  all  cross  products  required  in 
the  main  program.  Subroutine  (LEQT2F)  was  then  called  from 
the  IMSL  library.  This  subroutine  takes  MatA  inverts  it, 
multiplies  it  by  MatB  and  solves  the  generalized  equation 
A{=b   for   the   b   vector   using   Gaussian  elimination  with 
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iterative  improvement  to  get  accuracy  within  six  decimal 
digits.  The  output  from  LEQT2F  returns  from  the  subroutine 
via  MatB.  This  output  is  then  used  by  the  INTGRL  DSL 
statement  to  take  the  integral  of  angular  acceleration 
(wdx,  wdy,  wdz)  to  get  angular  velocity  (wx,  wy,  wz)  and 
again  to  get  the  position  of  the  link  with  respect  to  theta 
(c6x,  cdy,  cdz)  for  each  torque  input  per  time  step.  The 
cartesian  orientations  are  converted  to  Euler  angles  (6x, 
ey,  9z)  prior  to  returning  to  the  beginning  of  the  program. 
The  method  used  to  solve  the  second  order  differential, 
equation  for  accelerations  is  invoked  by  ADAMS  which  is  the 
second  order,  variable  step  integration  ADAMS  method.  This 
method  was  shown  to  be  the  fastest  (CPU  time)  and  most 
accurate  of  the  methods  available  [Ref.  8].  Similarly, 
INTGRL  is  applied  to  find  the  linear  acceleration  (A)  of 
each  link,  velocity  (V,)  and  finally  the  position  of  the 
center  of  gravity  of  the  link.  These  newly  found  values  are 
fed  back  into  the  beginning  of  the  simulation  program  for 
the  next  time  step  until  the  end  of  the  interval.  This 
process  is  summarized  in  Figure  3. 
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Constraints  are  also  built  into  the  simulation  program 
that  enable  the  operator  to  limit  the  movement  of  the  links 
in  the  yz  plane  for  a  two  dimensional  demonstration  of 
link-three-only  movement.  The  constraints  are  also  used  for 
link  two  and  three  movement.  The  constraints  are  applied  by 
zeroing  out  a  row,  except  for  the  diagonal  which  is  set  to 
1.0.  The  MatB  entry  is  set  to  the  constrained  value. 

It  is  during  this  simulation  that  a  differentiation 
should  be  made  between  the  cartesian  theta  (cd)  position 
developed  by  the  INTGRL  function  (Figure  4a)  and  the  Euler 
angler  theta  (6)  used  as  direction  angles  in  computing 
distances  (Figure  4b).  When  in  the  yz  plane  the  angular 
acceleration  is  about  the  z  axis  and  when  the  integral  is 
taken  twice  with  respect  to  tine,  what  results  is  the  angle 
theta  about  the  x  axis.  This  cartesian  angle  is  defined  as 
cdx  and  is  obviously  not  the  same  as  the  theta  angle  used 
to  position  the  link  initially  which  is  defined  as  8y.  To 
resolve  this  discrepancy  when  cdx  is  computed  it  is 
converted  to  the  euler  angle  8y  by  setting  the  two  equal  so 
6y=c0x,  in  a  two-dimensional  simulation.  Additionally, 
euler  angle  ez=»90<>-ey  and  8x*90<>  whenever  simulating,  two- 
dimensional  yz  plane  motions. 
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Figure  4.  Angles  of  Orientation  (a)  Cartesian  angles  (b)  Euler  angles 
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B.   PROGRAM  VALIDATION 

Validation  of  the  simulation  program  takes  place  in  two 
ways. 

1.  Validation  of  One  Link  Case 

For  link  three  the  theoretical  value  of  theta  in  the 
x  direction  (th0x3),  is  compared  to  the  value  of  theta  in 
the  x  direction  (6x3)  that  the  simulation  computed  for  each 
time  step  (Appendix  A  has  the  program  listing). 

As  a   test  case, the   torque  delivered   at  joint  two  was 
assumed  to  be: 

T2x=10sin(2«t). 
Also,  Ixxa,  the  moment  of  inertia  about  joint  two,  is  equal 
to  mass   at  the   end  of   the  link  M(3,2)  times  the  distance 
from  joint  two  to  the  mass  at  the  end  of   the  link,  squared 
or, 

Ixxa=M(3,2)  x  (L(3,2)  +  L(3,D)  . 

This  may   be  used   to  solve  for  th6x  by  taking  the  integral 

with  respect  to  the  time  which  results  in: 

th6x3=T2x 
Ixxa 

,t 
ft  th8x3  dt=-10     cos  (2xt) 

Jo            Ixxa  (2*)         lo 
thex3«(-10    (cos2xt)  +  IQ )  1 


2k  2x  Ixxa 


r 


3  dt=  th6x3=(-10     sin(2xt)  +  lOt   )1 +  i 

4***2  2x     Ixxa    4 
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For  comparison  %   error  is  used  so 

%  error«(th63-ex3)  x  100 
max  thdx3 

2.  Validation  of  Two  Links  Case 

For  the  validation  of  two  links  the  computed  torques 
at  joints  two  and  joint  one  (Tory2x,  Torylx)  are  compared 
to  the  torques  that  are  actually  input  (T2x,  Tlx)  at  each 
time  step  (Appendix  B  has  the  program  listing).  If  there 
are  no  effects  of  singularity  then  the  theoretical  torque 
and  input  torque  should  be  very  similar.  From  Figure  2  the 
sum  of  the  moments  about  the  center  of  gravity  of  link 
three  is: 

£H3=M3(L(3,2)2(wdx(3))=T2x  +  Fz2y  -  Fy2z 
so 

T2x=M3(L(3,2))2(wdx(3))  -  Fz2y  +  Fy2z=Tory2x 
where 

y=L(3,l)(cos(6y3)) 

Fy2=(-M3)(ay3) 

z=L(3,l)(cos(8y3)) 

Fz2=(-M3)(az3) 
Sum  of  the  moments  about  the  center  of  gravity  of   link  two 
is: 

EM2«Ixx2(wdx(2) )=Tlx-T2x  +  Fzlcos(8y2) (L( 2, 1) ) 

-Fylsin(6y2)(L(2,l))  +  Fz2cos(6y2) (L( 2, 2) ) 

-Fy2sin(9y2)(L<2,2)) 
so 
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Tlx=M2(L(2,2) )2(wdx(2) )  +  T2x-Fzlcos(6y2) (L( 2, 1 ) ) 
+  Fylsin(ey2)(L<2,l))-Fz2(L(2,2))cos(6y2> 
+  Fy2sin(0y2)(L(2,2))=Torylx 
where 

Fzl=Fz2-M2(az2) 
Fyl=Fy2-M2ay2. 
For  comparison  the  %   error  is   used  for   difference  in 
torque  for  joint  two  and  joint  one: 

Errt2x=(Tory2x-T2x)/(maxTory2x)  x  100 
Errtlx-(Torylx-Tlx)/(maxTorylx)  x  100 
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A.   MOVEMENT  OF  LINK  THREE 

Analysis  of  the  movement  of  only  link  three  shows  very 
good  results  for  program  validation.  Figure  5  shows  a  plot 
of  Euler  angles  for  both  theoretical  (they 3)  and  simulated 
(6y3)  values,  the  graph  shows  indistinguishable 
differences.  To  further  visualize  the  difference  Figure  6 
was  plotted,  which  is  the  %  error  between  they  and  dy 
versus  time.  There  seems  to  be  greater  error  (0.00  32%)  at 
around  0.8  seconds  than  at  0.2  seconds.  This  could  possibly 
be  caused  by  error  buildup  in  the  computation  due  to  round 
off  error  from  subroutine  LEQT2F  and  truncation  error  from 
approximating  the  solution  to  the  second  order  differential 
equation  by  the  ADAMS  method.  Additionally,  inaccuracies 
could  occur  in  estimating  the  value  of  %  and  using  it  in 
trigonometric  calculations . However, the  %  error  is  small  and 
is  acceptable  to  verify  the  proper  operation  of  the  program 
for  the  single  degree  of  freedom  case. 
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B.   MOVEMENT  OF  LINK  TWO  AND  THREE 

Analysis  of  the  movement  of  link,  two  and  three  is  the 
crucial  test  of  how  the  simulation  deals  with  the  problem 
of  singularity.  A  torque  was  input  to  joint  two  and  an 
opposite  torque  to  joint  one.  At  some  point  the  alignments 
of  the  two  links  will  have  some  absolute  angle  of  0° 
(Figure  7)  relative  to  each  other.  At  this  time  if 
singularity  exists  there  is  no  longer  any  control  of  the 
links  and  accelerations  and  velocities  vary  abnormally, 
never  returning  to  the  level  they  were  at  before  the 
singular  position  was  reached  [Ref.  8].  So  the  reason  for 
comparing  the  values  of  the  computed  torques  (Tory2x, 
Torylx)  given  the  position  variables  solved  for  by  the 
simulation  program  and  the  torques  input  to  the  joints 
(T2x,  Tlx),  is  to  check  for  abnormalities.  Figure  8  shows 
the  graph  of  computed  and  input  torques  for  joint  two  and 
Figure  9  shows  the  graph  of  computed  and  input  torques  for 
joint  one  versus  time.  The  two  curves  match  very  well  and 
shows  almost  no  deviation  between  them  for  the  scale  used. 

When  the  %  errors  are  plotted  between  computed  torque 
and  input  torque  versus  time  for  links  two  and  one  (Figures 
10  and  11)  again  very  little  %  error  is  observed  with  the 
largest  being  around  0.0  24%  at  time  2.8  seconds  for  torque 
input  at  joint  one.  This  may  be  attributed  to  the  similar 
reasons  as  the  one-link  since  now  both  link  two  and  three 
are  moving  these  errors  are  building  as  time   increases.  It 
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is  also  observed  in  Figure  10  that  the  %  error  is  not 
smooth  but  erratic  and  causes  a  "spikey"  curve  fit. 
However,  the  error  comes  back,  down  to  the  zero  plateau 
instead  of  remaining  at  a  high  level  which  is  what  would 
have  happened  had  singularity  occurred.  The  overall  % 
errors  are  small  and  so  lends  credabillty  to  the  simulation 
model.  Figure  7  was  plotted  to  see  at  what  point  the  two 
links  align  themselves  and  to  get  a  picture  of  about  how 
long  they  are  close  (within  one  degree)  to  the  point  of 
singularity.  It  appears  to  be  0 . 5  seconds  which  is  enough 
time  for  singularity  to  have  a  strong  effect  [Ref.  8]. 
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VI.  CONCLUSIONS 

The  ability  of  a  global  two  degree  of  freedom  robot  arm 
to  maneuver  through  a  point  of  singularity  under  applied 
torques  was  demonstrated.  This  was  verified  by  comparing 
the  computed  torque  at  joint  one  and  two  in  the  z  direction 
to  the  values  that  were  input.  There  were  no  unusual  or 
abnormal  results  occurring  in  the  acceleration  or 
velocities  and  so  little  error  was  produced. 
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VII.  RECOMMEMDATIOMS 

The  following  recommendations  are  provided: 

1.  Develop  a  linearized  manipulator  model  and  a 
corresponding  controller  for  the  two  degree  of  freedom 
case. 

2.  Validate  the  approach  via  actual  empirical  tests  for  the 
two  dimensional  case.  This  will  establish  the  difficulty  of 
determining  accurate  constants  for  the  simulation  and 
controller  design. 

3.  Demonstrate  the  model  and  controller  in  3  dimensions 
with  3  links.  The  difficulty  here  arises  in  analyzing  the 
direction  of  a  given  joint  torque  in  3  dimensions.  This 
could  probably  be  done  by  finding  a  unit  normal  vector 
perpendicular  to  the  joint  in  the  x,  y  and  z  direction  and 
multiplying  it  by  the  torque  magnitude. 

4.  Validate  the  approach  by  implementation  for  the  three 
dimensional  case. 
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APPENDIX  A 
SIMULATION  PROGRAM  FOR  MOVEMENT  OF  LINK  THREE 


TERMINAL 

METHOD  ADAMS 

PRINT  .01, ETHETY ( 3 ) , EULORY ( 3 ) , ERROR ( 3 ) 

CONTROL  FINTIM  =1.0,  DELMAX  =.01,  DELPRT  =  .01 

SAVE  .01,ETHETY(3) ,EUL0RY(3) ,ERR0R(3) 

GRAPH(DE=TEK618)  TIME , ETHETY(3) 

GRAPH(DE=TEK618)  TIME ,ERR0R(3) 

GRAPH(DE=TEK618)  TIME ,EUL0RY(3) 

D     DIMENSION  MATA(27.27) , MASS (3, 2) ,L(3, 2) ,RX(3 , 2) ,RY(3 , 2) ,RZ(3 , 2) 

D     D IMENS I ON  I XX ( 3 , 2 ) , I XZ ( 3 , 2 ) , I XY ( 3 , 2 ) , I YY ( 3 , 2 ) , I YZ ( 3 , 2 ) , IZZ ( 3 , 2 ) 

D     INTEGER  IER, I , RUN ,M,N, IA, IDGT 

EXCLUDE  IA,IDGT,IER,I,RUN,M,N 

ARRAY  MATB {21) . LCOGX ( 3 ) , LCOGY ( 3 ) , LCOGZ ( 3 ) , ETHETX ( 3 ) , ETHETY ( 3 ) , ETHETZ ( 3 ) 

ARRAY  CTHETX(3) ,CTHETY(3) ,CTHETZ(3) ,THDDOT(3) ,IXXA(3) ,ERROR(3) 

ARRAY  VECTA0(3) , VECTB0(3) . VECTA1 (3) ,VECTB1(3) ,VECTA2(3) ,VECTB2(3) 

ARRAY  WDX(3).WDY(3) ,WDZ(3) ,WX(3) ,WY(3) ,WZ(3) ,RBG1(3) ,RAG1(3) ,THEORY(3) 

ARRAY  RBG2(3) ,RAG2(3) ,RBG3(3) ,THETXR(3) ,THETYR(3) ,THETZR(3) ,EULORY(3) 

ARRAY  HDX(2) ,HDY(2) ,HDZ(2) 

ARRAY  SUMHDX(3) ,SUMHDY(3) ,SUMHDZ(3) ,WKAREA(850) 

D     DATA  MATA/729  *  0./ 


INITIAI 

l 

* 

INPUT  PARAMETER 

CONSTANTS 

A  =  10.0 

P  =  0 

.0 

W  =  2 

*PI 

IDGT 

=  4 

G=0.0 

N=27 

M=l 

IA  =27 

RUN  = 

1 

* 

INPUT  JOINT  LOCATIONS  IN  METERS 

JX0 

=  0, 

,0 

JY0 

=  0, 

,0 

JZO 

=  0, 

,0 

JX1 

=  0, 

,0 

JY1 

=  1, 

.0 

JZ1 

=  0, 

.0 

JX2 

=  0, 

.0 

JY2 

=  2 

.0 

JZ2 

=  0 

.0 

* 

INPUT  TORQUE 

CONSTANTS 

TOX 

=  0 

.0 

TOY 

=  0 

.0 

TOZ 

=  0 

.0 

T1X 

=  0 

.0 

T1Y 

=  0 

.0 

T1Z 

=  0 

.0 

T2Y 

=  0 

.0 

T2Z 

=  0 

.0 

*: 

[NPUT 

DISTANCE 

FROM  CENTER  OF  LINK  TO 

L( 

'1, 

■l)    z 

=  0, 

,50 

L< 

,1, 

2   : 

=  0, 

,50 

L< 

2 

■i)  : 

=  0, 

,50 

L< 

2 

■2)  : 

=  0, 

,50 

LI 

3, 

.1)  : 

=  0, 

.50 
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L(3,2)  =  0.50 
INPUT  MASS  AT  LINK  ENDS  IN  KILOGRAMS 


MASS! 

'1,1)  =  2.5 

MASS! 

1,2)  =  2.5 

MASSI 

2,1   =  2.5 

MASS! 

2,2)  =  2.5 

MASS< 

3,1)  =  2.5 

MASS* 

3,2)  =  2.5 

* 

INPUT  OMEGA  AND  OMEGA  DOT 

DO  30  I  =  1,3 

WX(I)  =  0.0 

WY(I)  =  0.0 

WZ(I)  =  0.0 

WDX(I)  =0.0 

WDY(l)  =0.0 

WDZ(I)  =0.0 

30 

CONTINUE 

* 

INPUT  TNT1 

7TAL  VALUES  OF  E 

ETHETX(l)  =90.0 
TX1  =  ETHETX(l)  *  DEGRA 
ETHETY(l)  =0.0 
TY1  =  ETHETY(l)  *  DEGRA 
ETHETZ(l)  =90.0 
TZ1  =  ETHETZ(l)  *  DEGRA 
ETHETX(2)  =  90.0 
TX2  =  ETHETX(2)  *  DEGRA 
ETHETY(2)  =  0.0 
TY2=  ETHETY(2)  *  DEGRA 
ETHETZ(2)  =90.0 
TZ2  =  ETHETZ(2)  *  DEGRA 
ETHETX(3)  =90.0 
TX3  =  ETHETX(3)  *  DEGRA 
ETHETY(3)  =45.0 
TY3  =  ETHETY(3)  *  DEGRA 
ETHETZ(3)  =45.0 
TZ3  =  ETHETZ(3)  *  DEGRA 

INPUT  LOCATION  OF  LINK  CENTERS  OF  GRAVITY 
LCOGX(l)  =0.0 
XI  =  LCOGX(l' 
LCOGY(l)  =  0 
Yl  =  LCOGY(l 
LCOGZ(l)  =  0 
Zl  =  LCOGZ(l 
LCOGX(2)  =  0 
X2  =  LCOGX(2 
LCOGY(2)  =  1 
Y2  =  LCOGY(2 
LCOGZ(2)  =  0 
Z2  =  LC0GZ(2 
LCOGX(3)  =  0 
X3  =  LCOGX(3 

THERAD  =  ETHETY(3)  *  DEGRA 
LCOGY(3)  =  2.0  +  COS(THERAD)  *  L(3,l) 
Y3  =  LCOGY(3) 

LCOGZ(3)  =  L(3,l)  *  SIN(THERAD) 
Z3  =  LCOGZ(3) 

INPUT  MASS  OF  EACH  LINK  IN  KG  AND  COMPUTE  WEIGHTS  IN  NEWTONS 
MASSI  =5.0 
MASS2  =5.0 
MASS3  =5.0 
Wl  =  MASSING 
W2  =  MASS2*G 
W3  =  MASS3*G 
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INPUT  ACCELERATION  OF  JOINT  ZERO 
AOX  =0.0 
AOY  =  0.0 
AOZ  =  0.0 


DERIVATIVE 


NOSORT 

*  INPUT  JOINT  EQUATIONS 

*  INITIALIZE  MATRIX  B  TO  ZERO 

DO  10  I  =  1,27 
MATB(I)  =0.0 
10         CONTINUE 

*  INPUT  TORQUE  AT  JOINTS 

T2X  =  A*SIN  (W*TIME  +P) 

+  (WD1  X  RB/G1)  +  Wl  X  (Wl  X  RB/G1) 

[1) 
1) 

LCOGX(l) 
LCOGY(l) 
LCOGZ(l) 
CALL  CPROD ( VECTAO, RBG1,MIA0,MJA0,MKA0) 
VECTAO(l)  =  WX(1) 
VECTA0(2)  =  WY(1) 
VECTAO (3)  =  WZ(1) 
CALL  CPROD ( VECTAO , RBG1 , MIBO , MJBO , MKBO ) 
VECTBO(l)  =  MIBO 
VECTB0(2)  =  MJBO 
VECTB0(3)  =  MKBO 
CALL  CPROD (VECTAO , VECTBO , MICO , M JCO , MKCO ) 


JOINT 

ZERO  i 

\E 

=  AG1 

VECTAO i 

(1) 

=  WDX 

VECTAO i 

2) 

=  WDY 

VECTAO i 

[3) 

=  WDZ 

RBG1(1' 

)  = 

JXO  - 

RBG1(2' 

)  = 

JYO  - 

RBG1(3' 

)  = 

JZO  - 

JOINT  ONE  EQUATIONS 
VECTA1 
VECTA1 
VECTA1 
RAG1(1 
RAG1(2 
RAG1(3 


AA  =  AG1 
=  WDX(l) 
=  WDY 
=  WDZ 
JX1  - 
JY1  - 
JZ1 


+  (WD1  X  RA/G1)  +  Wl  X  (Wl  X  RA/G1) 


1) 
1) 

LCOGX(l) 
LCOGY(l) 
LCOGZ(l) 
CALL  CPROD (VECTA1 ,RAG1 ,MIA1 ,MJA1 ,MKA1) 
VECTAl(l)  =  WX(1) 
VECTA1(2)  =  WY(1) 
VECTA1(3)  =  WZ(1) 
CALL  CPROD  (VECTA1,RAG1,MIB1,MJB1,MKB1) 
VECTBl(l)  =  MIB1 
VECTB1(2)  =  MJB1 
VECTB1(3)  =  MKB1 
CALL  CPROD  (VECTA1,VECTB1,MIC1,MJC1,MKC1) 

AB  =  AG2  +  (WD2  X  RB/G2)  +  W2  X  (W2  X  RB/G2) 
VECTA1'" 


1)  =  WDX( 
VECTA1(2)  =  WDY  I 
VECTA1(3)  =  WDZ  I 
RBG2(1)  =  JX1  - 
RBG2(2)  =  JY1  - 
RBG2(3)  =  JZ1 


2 

'2) 
2) 

LCOGX(2) 
LCOGY(2) 
LCOGZ(2) 

CALL  CPROD  (VECTA1,RBG2,MIA2,MJA2,MKA2) 
VECTAl(l)  =  WX(2) 
VECTA1(2)  =  WY(2) 
VECTA1(3)  =  WZ(2) 
CALL  CPROD  (VECTA1,RBG2,MIB2,MJB2,MKB2) 
VECTBl(l)  =  MIB2 
VECTB1(2)  =  MJB2 
VECTB1(3)  =  MKB2 
CALL  CPROD  (VECTA1,VECTB1,MIC2,MJC2,MKC2) 
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*  JOINT   TWO   E( 

*  AA  =   AG; 


lUATIONS 

i   +    (WD2   X   RA/G2)    +   W2   X    (W2   X  RA/G2) 

VECTA2(1)  =  WDX(2) 

VECTA2(2)  =  WDY(2) 

VECTA2(3)  =  WDZ(2) 

RAG2(1)  =  JX2  -  LCOGX(2) 

RAG2(2)  =  JY2  -  LCOGY(2) 

RAG2(3)  =  JZ2  -  LCOGZ(2) 
CALL  CPROD  (VECTA2,RAG2,MIA3,MJA3,MKA3) 

VECTA2(1)  =  WX(2) 

VECTA2(2)  =  WY(2) 

VECTA2(3)  =  WZ(2) 
CALL  CPROD  (VECTA2,RAG2,MIB3,MJB3,MKB3) 

VECTB2(1)  =  MIB3 

VECTB2(2)  =  MJB3 

VECTB2(3)  =  MKB3 
CALL  CPROD ( VECTA2 , VECTB2 , MI C3 , M JC3 , MKC3 ) 


AB  =  AG3  +  (WD3  X  RB/G3)  + 
VECTA2(1)  =  WDX( 
VECTA2(2)  =  WDY< 
VECTA2(3)  =  WDZ( 
RBG3(1)  =  JX2  - 
=  JY2  - 


W3  X  (W3  X  RB/G3) 

3) 

3) 

3) 

LCOGX(3) 

LCOGY(3) 


RBG3(2 

RBG3(3)  =  JZ2  -  LCOGZ(3) 
CALL  CPROD  (VECTA2,RBG3,MIA4,MKA4,MKA4) 

VECTA2(1)  =  WX(3) 

VECTA2(2)  =  WY  3) 

VECTA2(3)  =  WZ(3) 
CALL  CPROD  (VECTA2,RBG3,MIB4,MJB4,MKB4) 

VECTB2(1)  =  MIB4 

VECTB2(2)  =  MJB4 

VECTB2(3)  =  MKB4 
CALL  CPROD  (VECTA2,VECTB2,MIC4,MJC4/MKC4) 

SUM  OF  MOMENTS  EQUATIONS 

CONVERT  EULER  ANGLES  FROM  DEGREES  TO  RADIANS 
DO  40  I  =  1,3 

THETXR(I)  =  ETHETX(I)  *  DEGRA 
THETYR(I)  =  ETHETY(I)  *  DEGRA 
THETZR(I)  =  ETHETZ(I)  *  DEGRA 


COMPUTE  HX  DOT,HY  DOT, 


I XX 
I XX 
IXZ 
IXZ 
IXY 


RXl 
RXi 
RYl 
RYl 
RZi 
RZI 
.1 


=  L 
=  L 


=  L 
1,1 
1,2 

1,1 


=MASS 

2)=MASS 

l)=MASS 

2)=MASS(I,2 

1)=MASS(I,1 

IXY(I,2)=MASS(I,2 

HDX(l)  =  WDX 

HDX(2)  =  WDX 

IYY(I,1)  =  MASS(I 

IYY(I,2)  =  MASS(I 

IYZ(I,1)  =  MASS(I 

IYZ(I,2)  =  MASS (I 

HDY(l)  =  WDY 

HDY(2)  =  WDY 

IZZ(I,1)  =  MASS (I 

IZZ(I,2)  =  MASS(I 

HDZ(l)  =  WDZ 

HDZ(2)  =  WDZ 

IXXA(I)=MASS(I,2) 

SUMHDX(I)  =  HDX(1 


HZ  DOT 
L(I,1)  *  COS 
(1,2)  *  COS( 
L(I,1)  *  COS 
(1,2)  *  COS( 
L(I,1)  *  COS 
(1,2)  *  COS( 

*((RY 

*((RY 

*  RZ 

*  RZ 

*  RX 

*  RX 
1)*IXX(I 

*IXX(I 

*  ((RX(i 

*  ((RX(I 

*  RY(I,1 

*  RY(I,2 
*IYY(I,1 
*IYY 

*IZZ 
*IZZ 
((L(I,2)+L'( 
)  +  HDX(2) 


(THETXR( 
THETXR(I 
(THETYR( 
THETYR(I 
(THETZR( 
THETZR(I 
RY?I,1  ) 
RY(I,2  ) 


:i 


1,2 
'RX('I 
RX(I 

,1/1' 

1,2 


*  RX 

*  RX 

*  RY 

*  RY 
-WDZ 
-WDZ 

,1)*RX 
2)*RX 

*  RZ 

*  RZ 
WDX  (I 
WDX  (I 

,1)*RX(I 
,2)*RX(I 
-WDX(I)* 
-WDX(I)* 
I,D)**2 


,1,1 

,1,2 

1,1 

:i,2 
n* 

i)* 

i 

i 
i 

i 

* 


) 

RZ  (I ',  2)*RZ(  I 


l)*: 


M 


IXZ (1,1 
IXZ (I, 2 

+ 
+ 


,1, 
,2, 
,1 
,2' 

IXY  i 
IXYi 


-WDY (I 
-WDY(I' 

;rzji,i; 

RZ(I,2 


)-WDX(I)*IXY(I,l) 
)-WDX(I)*IXY(I,2) 

)*IXZ(I,1) 
)*IXZ(I,2) 


■WDZ (I 
■WDZ (I 

;ry(i,i 

:ry(i,2 

■WDY(I 
>-WDY(I 


*IXY(I,1 
*IXY(I,2' 
*RZ(I,1) 
*RZ(I,2 


*IYZ(I,1 

*IYZ(I,2' 

*RY ( 1 , 1 ) ' 

*RY(I,2) 

*IYZ(I,1' 

*IYZ(I,2 


60 


40 


* 

1 

60 

2 

70 

* 
* 

3 


SUMHDY(I)  =  HDY(1 

SUMHDZ(I)  =  HDZ(1 

CONTINUE 


)  +  HDY(2) 
)  +  HDZ(2) 


IN  EFFECT  1,2  OR  3 


TEST  TO  SEE  WHICH  CONSTRAINT  IS 
IF  (RUN  .EQ.  1)  GO  TO  1 
IF  (RUN  .EQ.  2)  GO  TO  2 
IF  (RUN  .EQ.  3)  GO  TO  3 

INITIAIIZE  MATRIX  ACCORDING  TO  CONSTRAINT 
DO  60  I  =  1,18 

MATA(I,I)  =  1.0 
CONTINUE 
GO  TO  4 
DO  70  I  =  1,9 

MATA(I,I)  =1.0 
CONTINUE 
GO  TO  7 

ENTER  CONSTANTS  INTO  MATRIX  A 
LINK  ONE 

SUM  OF  FORCES  IN  THE  X  DIRECTION 
MATA(1,1)  =1.0 

MATA(1,4)  =  MASS1 
MATA(1,10)  =  -1.0 

SUM  OF  FORCES  IN  Y  DIRECTION 
MATA(2,2)  =1.0 
MATA (2, 5)  =  MASS1 
MATA(2,11)  =  -1.0 

SUM  OF  FORCES  IN  Z  DIRECTION 


MATA(3,3)  =  1.0 

)  =  MASS1 


MATA 
MATA 


3,12)  =  -1.0 


SUM  OF  FORCES  LINK  ONE  EQUAL 
MATB(3)  =  -Wl 

EQUATIONS  AT  JOINT  ZERO 
IN  THE  X  DIRECTION 
MATA(4,4)  =  1.0 
MATA(4,8)  =  RBG1(3) 
MATA(4,9)  =  -RBG1(2) 

MATB(4)=  AOX  -  MICO 

IN  THE  Y  DIRECTION 


MATA(5,5' 
MATA (5,7' 
MATA(5,9' 


=  1.0 

=  -RBG1(3) 

=  RBG1(1) 


MATB(5)  =  AOY  -  MJCO 
IN  THE  Z  DIRECTION 


MATA(6,6 

MATA(6,7' 

MATA(6,8' 


=  1.0 

=  RBG1(2) 

=  -RBG1(1) 


MATB(6)  =  AOZ  -  MKCO 

SUM  OF  MOMENTS  EQUATIONS  FOR  LINK  ONE  IN  THE  X,Y,Z  DIRECTIONS 

MATA(7,2)  =  RBG1(3) 

MATA(7,3   =  -RBG1(2) 

MATA(7,7)  =  -(IXX(l.l)  +  IXX(1.2)) 

MATA(7,8)  =  IXY(1,1)  +  IXY(1,2 

MATA(7,9)  =  IXZ(1,1)  +  IXZ(1,2 

MATA(7,11)  =  -RAG1(3) 
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MATA(7,12)  =  RAG1(2) 

MATB(7)  =  T1X  -  TOX 

MATA(8,1)  =  -RBG1(3) 

MATA(8,3)=  RBG1(1) 

MATA(8,7)  =  IXY(1,1)  +  IXY(1,2) 

MATA(8,8)  =  -(IYY(l.l)  +  IYY(1,2)) 

MATA(8,9)  =  IYZ(l,l)  +  IYZ(1,2) 

MATA(8,10)  =  RAG1(3) 

MATA(8,12)  =  -RAG1(1) 

MATB(8)  =  T1Y  -  TOY 

MATA(9,1)  =  RBG1(2) 

MATA(9,2)  =  -RBG1(1) 

MATA(9,7)  =  IXZ(1,1)  +  IXZ(1,2) 

MATA(9,8)  =  IYZ(1,1)  +  IYZ(1,2) 

MATA(9,9)  =  -(IZZ(l.l)  +  IZZ(1,2)) 

MATA(9,10)  =  -RAG1(2) 

MATA(9,11)  =  RAG1(1) 

MATB(9)  =  T1Z  -  TOZ 

*  LINK  TWO 

*  SUM  OF  FORCES  IN  X  DIRECTION 
7       MATA(IO.IO)  =  1.0 


MATA(lO,13)  =  MASS2 
MATA(10,19)  =  -1.0 


SUM  OF  FORCES  IN  THE  Y  DIRECTION 
MATA(11,11)  =  1.0 
MATA(11,14)  =  MASS2 
MATA(11,20)  =  -1.0 

SUM  OF  FORCES  IN  THE  Z  DIRECTION 
MATA(12,12)  =1.0 
MATA(12,15)  =  MASS2 
MATA(12,21)  =  -1.0 

SUM  OF  FORCES  LINK  TWO  EQUAL 
MATB(12)  =  -W2 


* 

EQUATIONS  AT  JOINT  ONE 

* 

IN  THE  X  DIRECTION 

MATA(13,4)  =  -1.0 

MATA(13,8)  =  -RAG1(3) 

MATA(13,9)  =  RAG1(2) 

MATA(13,13)  =  1.0 

MATA(13,17)  =  RBG2(3) 

MATA(13,18)  =  -RBG2(2) 

MATB(13)  =  MIC1  -  MIC2 

* 

IN  THE  Y  DIRECTION 

MATA(14,5)  =  -1.0 

MATA(14,7)  =  RAG1(3) 

MATA(14,9)  =  -RAG1(1) 

MATA(14,14)  =  1.0 

MATA(14,16)  =  -RBG2(3) 

MATA(14,18)  =  RBG2(1) 

MATB(14)  =  MJC1  -  MJC2 

IN  THE  Z  DIRECTION 

MATA(15,6)  =  -1.0 
MATA(15,7)  =  -RAG1(2) 
MATA(15,8)  =  RAG1(1) 
MATA(15,15)  =  1.0 
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MATAI 

;i8. 

,10) 

MATA< 

18, 

,11) 

MATAI 

18. 

,16 

MATAI 

18. 

,17) 

MATAI 

18. 

,18) 

MATAI 

18. 

,19 

MATAI 

18, 

,20) 

MATA(15,16)  =  RBG2(2) 
MATA(15,17)  =  -RBG2(1) 

MATB(15)  =  MKC1  -  MKC2 

SUM  OF  MOMENTS  EQUATIONS  FOR  LINK  TWO  IN  THE  X,Y,Z  DIRECTIONS 
MATA(16,11)  =  RBG2(3) 
MATA(16,12   =  -RBG2(2) 
MATA(16,16)  =  -(IXX(2.1)  +  IXX(2,2)) 
MATA(16,17)  =  IXY(2,1)  +  IXY(2,2 
MATA  16,18)  =  IXZ(2,l)  +  IXZ(2,2' 
MATA(16,20)  =  -RAG2(3) 
MATA(16,21)  =  RAG2(2) 
MATB(16)  =  -T1X  +  T2X 
IF(RUN  .EQ.  2)  GO  TO  11 

MATA(17,10)  =  -RBG2(3) 
MATA(17,12)  =  RBG2(1) 
MATA(17,16)  =  IXY(2,1)  +  IXY(2,2) 
MATA(17,17)  =  -(IYY(2.1)  +  IYY(2.2)) 
MATA(17,18)  =  IYZ(2,1)  +  IYZ(2,2) 
MATA (17, 19)  =  RAG2(3) 
MATA(17,21)  =  -RAG2(1) 

MATB(17)  =   -  T1Y  +  T2Y 

=  RBG2(2) 

=  -RBG2(1) 

=  IXZ(2,1)  +  IXZ(2,2) 

=  IYZ(2,1)  +  IYZ(2,2) 

=  -(IZZ(2.1)  +  IZZ(2,2)) 

=  -RAG2(2) 

=  RAG2(1) 

MATB(18)  =   -  T1Z  +  T2Z 

IF  (RUN  .EQ.  3)  GO  TO  4 
11       MATA(17,17)  =  1.0 

MATA(18,18)  =  1.0 

*  LINK  THREE 

*  SUM  OF  FORCES  IN  THE  X  DIRECTION 
4       MATA(19,19)  =  1.0 

MATA(19,22)  =  MASS3 

*  SUM  OF  FORCES  IN  THE  Y  DIRECTION 

MATA(20,20)  =  1.0 
MATA(20,23)  =  MASS3 

*  SUM  OF  FORCES  IN  THE  Z  DIRECTION 

MATA(21,21)  =  1.0 
MATA(21,24)  =  MASS3 

MATB(21)  =  -W3 

*  EQUATIONS  AT  JOINT  TWO 

*  IN  THE  X  DIRECTION 

MATA(22,13)  =  -1.0 

MATA(22,17)  =  -RAG2(3) 

MATA(22,18)  =  RAG2(2) 

MATA(22,22)  =  1.0 

MATA  22,26)  =  RBG3(3) 

MATA(22,27)  =  -RBG3(2) 

MATB(22)  =  MIC3  -  MIC4 

*  IN  THE  Y  DIRECTION 

MATA(23,14)  =  -1.0 
MATA(23,16)  =  RAG2(3) 
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MATA(23,18' 
MATA(23,23' 
MATA(23,25' 
MATA(23,27' 


=  -RAG2(1) 
=  1.0 

=  -RBG3(3) 
=  RBG3(1) 


MATB(23)  =  MJC3  -  MJC4 

IN  THE  Z  DIRECTION 

=  -1.0 

=  -RAG2(2) 

=  RAG2(1) 

=  1.0 

=  RBG3(2) 

=  -RBG3(1) 

MATB(24)  =  MKC3  -  MKC4 


MATA< 

'24, 

,15) 

HATA< 

24, 

,16) 

MATA< 

24, 

,17) 

MATA< 

!24< 

,24 

MATA< 

24, 

,25 

MATA< 

,24, 

,26) 

SUM  OF  MOMENTS  E 
MATA(25,20 
MATA(25,21 
MATA(25,25 
MATA(25,26 
MATA(25.27/ 
MATB (25)  =  -  T2X 
IF(RUN  .EQ.  1  .OR. 


UATIONS  FOR  LINK  THREE  IN  THE  X,Y,Z  DIRECTIONS 
=  RBG3(3) 
=  -RBG3(2) 
=  -(IXX(3.1)  +  IXX(3,2)) 

IXY(3,1)  +  IXY(3,2 

IXZ(3,l)  +  IXZ(3,2! 

RUN  .EQ.  2)  GO  TO  12 


MATA< 

;26 

,19) 

MATAI 

26 

,21 

MATA( 

26 

,25 

MATA< 

26 

,26 

MATA< 

!26, 

,27) 

-RBG3(3) 

RBG3(1) 
IXY(3,1)  + 
-(IYY(3  1) 
IYZ(3,1)  + 


MATB(26)  =  -  T2Y 


MATA(27,19 

MATA(27/20' 

MATA(27,25' 

MATA(27,26' 

MATA(27,27 


=  RBG3(2) 
=  -RBG3(1) 
=  IXZ(3,1)  + 
=  IYZ(3,l)  + 
=  -(IZZ(3,1) 


IXY(3,2) 
+  IYY(3.2)) 
IYZ(3,2) 


IXZ(3,2) 
IYZ(3,2) 
+  IZZ(3,2)) 
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MATB(27)  =  -  T2Z 

IF  (RUN  .EQ.  3)  GO  TO  13 
MATA(26,26)  =  1.0 
MATA(27,27)  =  1.0 


*  CALL  EQUATION  SOLVER  PROGRAM  FROM  IMSL 

13    CALL  LEQT2F(MATA,M,N,IA,MATB,IDGT,WKAREA,IER) 
IF  (IER  .NE.  0)  CALL  ENDJOB 

*  FIND  LCOGX,LCOGY,LCOGZ,THETA  VALUES ,WX,WY,WZ 

IF(RUN  .EQ.  1)  GO  TO  6 
IF  (RUN  .EQ.  2)  GO  TO  9 


LINK  ONE 

AX1  = 

VELX1 

LCOGX1 

LCOGX( 

AY1  = 

VELY1 

LCOGY1 

LCOGY( 

AZ1  = 

VELZ1 

LCOGZ1 

LCOGZ( 

WD1X  = 


MATB(4) 

=  INTGRL(0,AX1) 

=  INTGRL(X1,VELX1) 
1)  =  LCOGXl 
MATB ( 5 ) 
=  INTGRL(0,AY1) 

=  INTGRL(Y1,VELY1) 
1)  =  LCOGY1 
MATB (6) 
=  INTGRL(0,AZ1) 

=  INTGRL(Z1,VELZ1) 
1)  =  LCOGZl 

MATB (7) 
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W1X  =  INTGRL(0,WD1X) 

THEXR1  =  INTGRL(TY1,W1X) 

JXO=  LCOGX(l)  -  L(l,l)  *  COS(TXl) 

WDX(l)  =  WD1X 

WX(1)  =  W1X 

CTHETX(l)  =  THEXR1  *  RADEG 

ETHETY(l)  =  CTHETX(l) 

WD1Y  =  MATB(8) 

W1Y  =  INTGRL(0,WD1Y) 

THEYR1  =  INTGRL(0. ,W1Y) 

JYO  =  LCOGY(l)  -  L(l,l)  *  COS(THEXRl) 

WDY(l)  =  WD1Y 

WY(1)  =  W1Y 

CTHETY(l)  =  THEYR1  *  RADEG 

WD1Z  =  MATB(9) 

W1Z  =  INTGRL(0,WD1Z) 

THEZR1  =  INTGRL(0. ,W1Z) 

WDZ(l)  =  WD1Z 

WZ(1)  =  W1Z 

CTHETZ(l)  =  THEZR1  *  RADEG 

ETHETZ(l)  =  90.0  -  CTHETX(l) 

ETHEZ1  =  ETHETZ(l)  *  DEGRA 

JZO  =  LCOGZ(l)  -  L(l,l)  *  COS(ETHEZl) 

LINK  TWO 

AX2  =  MATB(13) 

VELX2  =  INTGRL(0,AX2) 

LC0GX2  =  INTGRL(X2,VELX2) 

LC0GX(2)  =  LC0GX2 

AY2  =  MATB(14) 

VELY2  =  INTGRL(0,AY2) 

LC0GY2  =  INTGRL(Y2,VELY2) 

LC0GY(2)  =  LC0GY2 

AZ2  =  MATB(15) 

VELZ2  =  INTGRL(0,AZ2) 

LC0GZ2  =  INTGRL(Z2,VELZ2) 

LC0GZ(2)  =  LC0GZ2 

WD2X  =  MATB(16) 

W2X  =  INTGRL(0,WD2X) 

THEXR2  =  INTGRL(TY2,W2X) 

JX1  =  LC0GX(2)  -  L(2,l)  *  C0S(TX2) 

WDX(2)  =  WD2X 

WX(2)  =  W2X 

CTHETX(2)  =  THEXR2  *  RADEG 

ETHETY(2)  =  CTHETX(2) 

WD2Y  =  MATB(17) 

W2Y  =  INTRGL(0,WD2Y) 

THEYR2  =  INTGRL(0. ,W2Y) 

JY1  =  LC0GY(2)  -  L(2,l)  *  C0S(THEXR2) 

WDY(2)  =  WD2Y 

WY(2)  =  W2Y 

CTHETY(2)  =THEYR2  *  RADEG 

WD2Z  =  MATB(18) 

W2Z  =  INTGRL(0,WD2Z) 

THEZR2  =  INTGRL(0. ;W2Z) 

WDZ(2)  =  WD2Z 

WZ(2)  =  W2Z 

CTHETZ(2)  =  THEZR2  *  RADEG 

ETHETZ(2)  =  90.0  -  CTHETX(2) 

ETHEZ2  =  ETHETZ(2)  *  DEGRA 

JZ1  =  LCOGZ(2)  -  L(2,l)  *  C0S(ETHEZ2) 

LINK  THREE 

AX3  =  MATB(22) 

VELX3  =  INTGRL(0. ,AX3) 

LC0GX3  =  INTGRL(X3,VELX3) 

LC0GX(3)  =  LC0GX3 

AY3  =  MATB(23) 

VELY3  =  INTGRL(0. ,AY3) 
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DYNAMIC 


LC0GY3  =  INTGRL(Y3,VELY3) 

LC0GY(3)  =  LC0GY3 

AZ3  =  MATB(24) 

VELZ3  =  INTGRL(0. ,AZ3) 

LC0GZ3  =  INTGRL(Z3,VELZ3) 

LC0GZ(3)  =  LC0GZ3 

WD3X  =  MATB(25) 

W3X  =  INTGRL(0. ,WD3X) 

THEXR3  =  INTGRL(TY3,W3X) 

JX2  =  LC0GX(3)  -  L(3,l)  *  C0S(TX3) 

WDX(3)  =  WD3X 

WX(3)  =  W3X 

CTHETX(3)  =  THEXR3  *  RADEG 

ETHETY(3)  =  CTHETX(3) 

WD3Y  =  MATB(26) 

W3Y  =  INTGRL(0. ,WD3Y) 

THEYR3  =  INTGRL(0. ,W3Y) 

JY2  =  LC0GY(3)  -  L(3,l)  *  C0S(THEXR3) 

WDY(3)  =  WD3Y 

WY(3)  =  W3Y 

CTHETY(3)  =  THEYR3  *  RADEG 

WD3Z  =  MATB(27) 

W3Z  =  INTGRL(0. ,WD3Z) 

THEZR3  =  INTGRL(0. ,W3Z) 

WDZ(3)  =  WD3Z 

WZ(3)  =  W3Z 

CTHETZ(3)  =  THEZR3  *  RADEG 

ETHETZ(3)  =  90.0  -  CTHETX(3) 

ETHEZ3  =  ETHETZ(3)  *  DEGRA 

JZ2  =  LCOGZ(3)  -  L(3,l)  *  COS(ETHEZ3) 


THEORY(3)=((((-2.5/(PI*PI))*SIN(W*TIME))+(5.*TIME)/PI)/IXXA(3)) 
+  PI/4. 
EUL0RY(3)  =  THE0RY(3)  *  RADEG 

THDD0T(3)  =  T2X/IXXA(3) 

ERROR(3)  =  ((ABS(EULORY(3)-ETHETY(3)))/  81.476)*100. 


END 

STOP 

FORTRAN 


SUBROUTINE  TO  COMPUTE  THE  CROSS  PRODUCT  OF  TWO  VECTOR 

SUBROUTINE  CPROD ( VECTA , VECTB , MI , M J , MK ) 
IMPLICIT  REAL*8  (A-Z) 
DIMENSION  VECTA(3),VECTB(3) 

MI  =  VECTA(2)  *  VECTB(3)  -  VECTA(3)  *  VECTB ( 
MJ  =  VECTA(3)  *  VECTB(l)  -  VECTA(l)  *  VECTB( 
MK  =  VECTA(l)  *  VECTB(2)  -  VECTA(2)  *  VECTB( 

RETURN 

END 
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APPENDIX  B 

SIMULATION  PROGRAM  FOR  MOVEMENT  OF  LINK  TWO  AND 

THREE 

TERMINAL 

METHOD  ADAMS 

PRINT  . 03 , T1X , T0RY1X , T2X , T0RY2X , ERRT2X , ERRT1X , ETHETY( 2-3 ) 

CONTROL  FINTIM  =3.0,  DELMAX  =.01,  DELPRT  =  .03 

SAVE  . 01 , ERRT2X, ERRT1X , TORY1X , TORY2X , T1X , T2X , ETHETY ( 2 ) , ETHETY ( 3 ) 

GRAPH(DE=TEK618)  TIME ,T1X,T0RY1X 

GRAPH (DE=TEK618)  TIME ,T2X,T0RY2X 

GRAPH (DE=TEK618)  TIME,ERRT2X 

GRAPH (DE=TEK618)  TIME,ERRT1X 

GRAPH ( DE=TEK6 18)  TIME , ETHETY ( 3 ) , ETHETY ( 2 ) 

D     DIMENSION  MATA(27 . 27 ) ,MASS (3 , 2) ,L(3. 2) ,RX(3 , 2) ,RY(3 , 2) ,RZ(3 , 2) 

D     DIMENSION  IXX(3 , 2) , IXZ(3 , 2) I  , IXY(3 , 2) , IYY(3 , 2) , IYZ(3 , 2) , IZZ(3 , 2) 

D     INTEGER  IER, I ,RUN,M,N, IA, IDGT 

EXCLUDE  I A, IDGT, IER, I, RUN, M,N 

ARRAY  MATB(27) ,LCOGX(3) ,LCOGY(3) ,LCOGZ(3) ,ETHETX(3) ,ETHETY(3) ,ETHETZ(3) 

ARRAY  CTHETX ( 3 ) , CTHETY ( 3 ) , CTHETZ ( 3 ) 

ARRAY  VECTA0(3),VECTB0(3),VECTA1(3) ,VECTB1(3) , VECTA2(3) ,VECTB2(3) 

ARRAY  WDX(3)/WDY(3) ,WDZ(3) ,WX(3) , WY(3) .WZ(3) ,RBG1 (3) ,RAG1(3) 

ARRAY  RBG2(3) ,RAG2(3) ,RBG3(3) ,THETXR(3) ,THETYR(3) ,THETZR(3) 

ARRAY  SUMHDX(3) ,SUMHDY(3) ,SUMHDZ(3) ,HDX(2) ,HDY(2) ,HDZ(2) ,WKAREA(850) 

D     DATA  MATA/729  *  0./ 

INITIAL 

*  INPUT  PARAMETER  CONSTANTS 

A  =  2.0 
P  =  0.0 
W  =  2*PI 
IDGT  =  4 
G=0.0 
N=27 
M=l 

IA  =27 
RUN  =  2 

*  INPUT  JOINT  LOCATIONS  IN  METERS 

JX0  =  0.0 
JYO  =  0.0 
JZO  =  0.0 
JX1  =  0.0 
JY1  =  1.0 
JZ1  =  0.0 
JX2  =  0.0 
JY2  =  2.0 
JZ2  =  0.0 

*  INPUT  TORQUE  CONSTANTS 

TOX  =0.0 
TOY  =0.0 
TOZ  =0.0 
T1Y  =  0.0 
T1Z  =  0.0 
T2Y  =0.0 
T2Z  =0.0 

*INPUT  DISTANCE  FROM  CENTER  OF  LINK  TO  CENTER  OF  MASS  FOR  EACH  LINK  ENDS 

L(l,l)  =  0.50 

L(l,2)  =  0.50 

L(2,l)  =  0.50 

L(2,2)  =  0.50 
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L(3,l)  =  0.50 
L(3,2)  =  0.50 

INPUT  MASS  AT  LINK  ENDS  IN  KILOGRAMS 

MASS (1,1)  =2.5 

MASS (1,2)  =2.5 

MASS (2,1)  =2.5 

MASS(2,2)  =  2.5 

MASS(3,1)  =  2.5 

MASS (3, 2)  =2.5 

INPUT  OMEGA  AND  OMEGA  DOT 
DO  30  I  =  1,3 
WX(I)  =  0.0 
WY(I)  =  0.0 
WZ(I)  =  0.0 
WDX(I)  =0.0 
WDY(I)  =0.0 
WDZ(I)  =0.0 


30  CONTINUE 


INPUT  INITIAL  VALUES  OF  EULER  ANGLE  THETA  AND  CONVERT  TO  RADIANS 
ETHETX(l)  =90.0 
TX1  =  ETHETX(l)  *  DEGRA 
ETHETY(l)  =0.0 
TY1  =  ETHETY(l)  *  DEGRA 
ETHETZ(l)  =90.0 
TZ1  =  ETHETZ(l)  *  DEGRA 
ETHETX(2)  =90.0 
TX2  =  ETHETX(2)  *  DEGRA 
ETHETY(2)  =0.0 
TY2=  ETHETY(2)  *  DEGRA 
ETHETZ(2)  =90.0 
TZ2  =  ETHETZ(2)  *  DEGRA 
ETHETX(3)  =  90.0 
TX3  =  ETHETX(3)  *  DEGRA 
ETHETY(3)  =45.0 
TY3  =  ETHETY(3)  *  DEGRA 
ETHETZ(3)  =45.0 
TZ3  =  ETHETZ(3)  *  DEGRA 

INPUT  LOCATION  OF  LINK  CENTERS  OF  GRAVITY 
LCOGX(l)  =0.0 
XI  =  LCOGX(l) 
LCOGY(l)  =0.5 
Yl  =  LCOGY(l) 
LCOGZ(l)  =0.0 
Zl  =  LCOGZ(l) 
LCOGX(2)  =0.0 
X2  =  LCOGX(2) 
LCOGY(2)  =1.5 
Y2  =  LCOGY(2) 
LCOGZ(2)  =0.0 
Z2  =  LCOGZ(2) 
LCOGX(3)  =0.0 
X3  =  LCOGX(3) 

THERAD  =  ETHETY(3)  *  DEGRA 
LCOGY(3)  =  2.0  +  COS(THERAD)  *  L(3,l) 
Y3  =  LCOGY(3) 

LCOGZ(3)  =  L(3,l)  *  SIN(THERAD) 
Z3  =  LCOGZ(3) 

INPUT  MASS  OF  EACH  LINK  IN  KG  AND  COMPUTE  WEIGHTS  IN  NEWTONS 
MASS1  =5.0 
MASS2  =5.0 
MASS3  =5.0 
Wl  =  MASSING 
W2  =  MASS2*G 
W3  =  MASS3*G 
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*  INPUT  ACCELERATION  OF  JOINT  ZERO 

AOX  =  0.0 
AOY  =0.0 
AOZ  =  0.0 

DERIVATIVE 

*  INPUT  JOINT  EQUATIONS 
NOSORT 

*  INITIALIZE  MATRIX  B  TO  ZERO 

DO  10  I  =  1,27 
MATB(I)  =0.0 
10         CONTINUE 

*  INPUT  TORQUE  AT  JOINTS 

T2X  =  -A*SIN  (W*TIME  +P) 
T1X  =  A*SIN  (W*TIME  +  P) 

*  JOINT  ZERO  AB  =  AG1  +  (WD1  X  RB/G1)  +  Wl  X  (Wl  X  RB/G1) 

VECTAO(l)  =  WDX(l) 

VECTA0(2)  =  WDY(l) 

VECTA0(3)  =  WDZ(l) 

RBG1(1)  =  JXO  -  LCOGX(l) 

RBG1(2)  =  JYO  -  LCOGY(l) 

RBG1(3)  =  JZO  -  LCOGZ(l) 
CALL  CPROD ( VECTAO , RBG1 , MIAO , MJAO , MKAO ) 

VECTAO(l)  =  WX(1) 

VECTA0(2)  =  WY(l) 

VECTAO (3)  =  WZ(1) 
CALL  CPROD ( VECTAO , RBG1 , MIBO , MJBO , MKBO ) 

VECTBO(l)  =  MIBO 

VECTB0(2)  =  MJBO 

VECTB0(3)  =  MKBO 
CALL  CPROD (VECTAO , VECTBO , MICO , MJCO , MKCO ) 


JOINT  ONE  EQUATIONS 
VECTA1 
VECTA1 
VECTA1 
RAG1(1 
RAG1(2 
RAG1(3 


AA  =  AG1  +  (WD1  X  RA/G1)  +  Wl  X  (Wl  X  RA/G1) 

=  WDX(l) 

=  WDY(l) 

=  WDZ(l) 

JX1  -  LCOGX(l) 

JY1  -  LCOGY(l) 

JZ1  -  LCOGZ(l) 
CALL  CPROD (VECTA1 ,RAG1 , MI Al ,MJA1 ,MKA1) 
VECTAl(l)  =  WX(1) 
VECTA1(2)  =  WY(1) 
VECTA1(3)  =  WZ(1) 
CALL  CPROD  (VECTA1,RAG1,MIB1,MJB1,MKB1) 
VECTBl(l)  =  MIB1 
VECTB1(2)  =  MJB1 
VECTB1(3)  =  MKB1 
CALL  CPROD  (VECTA1,VECTB1,MIC1,MJC1,MKC1) 

AB  =  AG2  +  (WD2  X  RB/G2)  +  W2  X  (W2  X  RB/G2) 

VECTAl(l)  =  WDX(2) 

VECTA1(2)  =  WDY(2) 

VECTA1(3)  =  WDZ(2) 

RBG2(1)  =  JX1  -  LCOGX(2) 

RBG2(2)  =  JY1  -  LCOGY(2) 

RBG2(3)  =  JZ1  -  LCOGZ(2) 
CALL  CPROD  (VECTA1,RBG2,MIA2,MJA2/MKA2) 

VECTAl(l)  =  WX(2) 

VECTA1(2)  =  WY(2) 

VECTA1(3)  =  WZ(2) 
CALL  CPROD  (VECTA1,RBG2,MIB2,MJB2,MKB2) 

VECTBl(l)  =  MIB2 

VECTB1(2)  =  MJB2 

VECTB1(3)  =  MKB2 


^9 


CALL  CPROD  (VECTA1,VECTB1,MIC2,MJC2,MKC2) 


*  JOINT  TWO  E( 

*  AA  =  ag; 


'UATIONS 
+  (WD2  X  RA/G2)  +  W2  X  (W2  X  RA/G2) 

VECTA2(1)  =  WDX(2) 

VECTA2(2)  =  WDY(2) 

VECTA2(3)  =  WDZ(2) 

RAG2(1)  =  JX2  -  LCOGX(2) 

RAG2(2)  =  JY2  -  LCOGY(2) 

RAG2(3)  =  JZ2  -  LCOGZ(2) 
CALL  CPROD  (VECTA2,RAG2,MIA3,MJA3,MKA3) 

VECTA2(1)  =  WX(2) 

VECTA2(2   =  WY(2) 

VECTA2(3)  =  WZ(2) 
CALL  CPROD  (VECTA2,RAG2,MIB3,MJB3,MKB3) 

VECTB2(1)  =  MIB3 

VECTB2(2)  =  MJB3 

VECTB2(3)  =  MKB3 
CALL  CPROD (VECTA2 , VECTB2 , MIC3 , M  JC3 , MKC3 ) 


VECTA2(1)  =  WDXi 
VECTA2(2)  =  WDYl 
VECTA2(3)  =  WDZi 
RBG3(1)  =  JX2 
RBG3(2)  =  JY2 
RBG3(3)  =  JZ2 


AB  =  AG3  +  (WD3  X  RB/G3)  +  W3  X  (W3  X  RB/G3) 

,3) 
3) 

LCOGX(3) 
LCOGY(3) 
LCOGZ(3) 
CALL  CPROD  (VECTA2,RBG3,MIA4,MKA4,MKA4) 
VECTA2(1)  =  WX(3) 
VECTA2(2)  =  WY(3) 
VECTA2(3)  =  WZ(3) 
CALL  CPROD  (VECTA2,RBG3,MIB4,MJB4,MKB4) 
VECTB2(1)  =  MIB4 
VECTB2(2)  =  MJB4 
VECTB2(3)  =  MKB4 
CALL  CPROD  (VECTA2,VECTB2,MIC4,MJC4,MKC4) 

SUM  OF  MOMENTS  EQUATIONS 

CONVERT  EULER  ANGLES  FROM  DEGREES  TO  RADIANS 
DO  40  I  =  1,3 

THETXR(I)  =  ETHETX(I)  *  DEGRA 
THETYR(I)  =  ETHETY(I)  *  DEGRA 
THETZR(I)  =  ETHETZ(I)  *  DEGRA 


COMPUTE 


IXX< 
IXX< 
IXZi 
IXZl 
IXYI 
IXYI 


HX,H 
RX 
RX 
RY 
RY 
RZ 
RZ 
,1 
.2 
,1 


DOT  X,HY, 
1,1' 


IYY 
IYY 

IYZ 
IYZ 


IZZ 
IZZ 


=  L 
=  L 


I 
I 
I 

1,2 
1,1 
1,2 
HDX 
HDX 
1,1 
1,2 
1,1 
1,2 
HDY 
HDY 

111) 

HDZ 
HDZ 


1,2 
=MASS(I,1 
=MASS(I,2 
=MASS(I,1 
=MASS(I,2 
=MASS(I,1 
=MASS (1,2 

(1)  =  WDX 

(2)  =  WDX 
=  MASS (I 
=  MASS (I 
=  MASS (I 
=  MASS (I 

(1)  =  WDY 

(2)  =  WDY 
=  MASS (I 
=  MASS (I 

=  WDZ 
WDZ 


H  DOT  Y.  HZ,H  DOT  Z 
L(I,1)  *  COS(THETXR( 
(1,2)  *  COS(THETXR(I 
L(I,1)  *  COS(THETYR( 
(1,2)  *  COS(THETYR(I 
L(I,1)  *  COS(THETZR( 
(1,2)  *  COS(THETZR(I 

I,l}*RY(I,l) 

I,2)*RY(I,2)) 

1,1 

1,2 

1,1 
2 


*((RY 
*  (RY 

*  RZ 

*  RZ 

*  RX 

*  RX 
l^IXX,. 

*IXX(l,2)-WDZ(l)* 

*  ((RX(I,1)*RX(I 

*  ((RX(I,2)*RX 

*  RY(I,l)  *  RZ 

*  RY(I,2)  *  RZ 


(«:" 


*RY(I 

*  RX(I,1 

*  RX(I,2' 

*  RY(I,1' 

*  RY(I,2' 
1,1) -WDZ (I)*] 

,2)-l 


*IYYI 
*IYY< 

*IZZl 
*IZZi 


1,1) 

1,2) 


■WDX  (I 

•WDX  (I 
RX('l,l)*RX(I 
RX(I,2)*RX(I 
I,l)-WDX(I)*IXZi 
I,2)-WDX(I)*IXZi 


I,1)*RZ(I 
I,2)*RZ(I 


,1)-WDY(I 
,2) -WDY (I 
+  (RZ(I,1 


■M 


RZ ( I , 2 ) *RZ ( I 


*IXY(I,1 
*IXY(I,2' 


■WDZ (I 
■WDZ (I 
+  (RY(I,1 

+  (RY(I,2 
,1)-WDY(I 
,2)-WDY(I 


*IYZ(I,1 

*IYZ(I,2' 

*RY(i,im 

'*RY(I,2)j) 

*IYZ(I,1 

*IYZ(I,2 


SUMHDX(I)  =  HDX(l)  +  HDX(2) 
SUMHDY(I)  =  HDY(l)  +  HDY(2) 
SUMHDZ(I)  =  HDZ(l)  +  HDZ(2) 
40       CONTINUE 

*  TEST  TO  SEE  WHICH  CONSTRAINT  IS  IN  EFFECT  1,2  OR  3 

IF  (RUN  .EQ.  1)  GO  TO  1 
IF  (RUN  .EQ.  2)  GO  TO  2 
IF  (RUN  .EQ.  3)  GO  TO  3 

*  INITIAIIZE  MATRIX  ACCORDING  TO  CONSTRAINT 

1  DO  60  I  =  1,18 

MATA(I,I)  =1.0 
60       CONTINUE 
GO  TO  4 

2  DO  70  I  =  1,9 

NATA(I,I)  =1.0 
70       CONTINUE 
GO  TO  7 

*  ENTER  CONSTANTS  INTO  MATRIX  A 

*  LINK  ONE 

*  SUM  OF  FORCES  IN  THE  X  DIRECTION 

3  MATA(1,1)  =1.0 

MATA(1,4)  =  MASS1 
MATA(1,10)  =  -1.0 

*  SUM  OF  FORCES  IN  Y  DIRECTION 

MATA(2,2)  =  1.0 
MATA(2,5)  =  MASS1 
MATA(2,11)  =  -1.0 

*  SUM  OF  FORCES  IN  Z  DIRECTION 

MATA(3,3)  =1.0 
MATA(3,6)  =  MASS1 
MATA(3,12)  =  -1.0 

*  SUM  OF  FORCES  LINK  ONE  EQUAL 

MATB(3)  =  -Wl 

*  EQUATIONS  AT  JOINT  ZERO 

*  IN  THE  X  DIRECTION 

MATA(4,4)  =1.0 
MATA(4,8)  =  RBG1(3) 
MATA(4,9)  =  -RBG1(2) 

MATB(4)=  AOX  -  MICO 

*  IN  THE  Y  DIRECTION 

MATA(5,5)  =1.0 
nMk(S,7)    =  -RBG1(3) 
MATA(5,9)  =  RBG1(1) 

MATB(5)  =  AOY  -  MJCO 

*  IN  THE  Z  DIRECTION 

MATA(6,6)  =  1.0 
MATA(6,7)  =  RBG1(2) 
MATA(6,8)  =  -RBG1(1) 

MATB(6)  =  AOZ  -  MKCO 

*  SUM  OF  MOMENTS  EQUATIONS  FOR  LINK  ONE  IN  THE  X,Y,Z  DIRECTIONS 

MATA(7,2)  =  RBG1(3) 

MATA(7,3)  =  -RBG1(2) 

MATA(7,7)  =  -(IXX(l.l)  +  IXX(1.2)) 

MATA(7,8)  =  IXY(1,1)  +  IXY(1,2, 

MATA(7,9)  =  IXZ(1,1)  +  IXZ(1,2' 
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MATA(7,11)  =  -RAG1(3) 
MATA(7,12)  =  RAG1(2) 

MATB(7)  =  T1X  -  TOX 


MATA(8,1)  =  -RBG1(3) 
MATA(8,3)=  RBG1(1) 

=  IXY(l.l)  + 
=  -(IYY(l.l) 


MATA(8,7 
MATA(8,8 
MATA(8,9 


MATA (8, 10)  =  RAG1(3 


IXY(1,2) 
+  IYY(1,2)) 


=  lYZ(l,'l)  +  IYZ(1,2J 

,10)  =  ■ 

MATA(8,12)  =  -RAGl(i) 


MATB(8)  =  T1Y  -  TOY 


MATA(9,1)  =  RBG1(2) 

MATA (9, 2)  =  -RBG1(1) 

MATA (9, 7)  =  IXZ(1,1)  +  \ 

MATA(9,8)  =  IYZ(l.l)  +  '. 

MATA  9,9)  =  -(IZZ(1,1)  - 

MATA(9,10)  =  -RAG1(2) 

MATA(9,11)  =  RAG1(1) 

MATB(9)  =  T1Z  -  TOZ 

* 

LINK  TWO 

* 

SUM  OF  FORCES  IN  X  DIRECTION 

7 

MATA(IO.IO)  =  1.0 

MATA(10,13)  =  MASS2 
MATA(10,19)  =  -1.0 

ixz(i,2; 

IYZ(1,2 

+  IZZ(1',2)) 


SUM  OF  FORCES  IN  THE  Y  DIRECTION 
MATA(11,11)  =  1.0 
MATA(11,14)  =  MASS2 
MATA(11,20)  =  -1.0 

SUM  OF  FORCES  IN  THE  Z  DIRECTION 
MATA(12,12)  =1.0 
MATA(12,15)  =  MASS2 
MATA(12,21)  =  -1.0 

SUM  OF  FORCES  LINK  TWO  EQUAL 
MATB(12)  =  -W2 

EQUATIONS  AT  JOINT  ONE 
IN  THE  X  DIRECTION 

=  -1.0 
=  -RAG1(3) 
=  RAG1(2) 
=  1.0 
=  RBG2(3) 
=  -RBG2(2) 

MATB(13)  =  MIC1  -  MIC2 

f  THE  Y  DIRECTION 

=  -1.0 
=  RAG1(3) 
=  -RAG1(1) 
=  1.0 
=  -RBG2(3) 
=  RBG2(1) 

MATB(14)  =  MJC1  -  MJC2 

IN  THE  Z  DIRECTION 

MATA(15,6)  =  -1.0 
MATA  15,7)  =  -RAG1(2) 
MATA(15,8)  =  RAG1(1) 


MATAI 

I13. 

■4) 

MATA< 

13' 

'8 

MATAI 

13^ 

.9) 

MATAI 

13. 

,13) 

MATAI 

13 

,17) 

MATAI 

!l3, 

,18) 

MATAI 

14, 

.5) 

MATAI 

14, 

■7) 

MATAI 

!4, 

9) 

MATAI 

14, 

,14) 

MATAI 

14, 

,16 

MATAI 

!l4, 

18) 
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MATA(15,15)  =  1.0 
MATA(15,16)  =  RBG2(2) 
MATA(15,17)  =  -RBG2(1) 

MATB(15)  =  MKC1  -  MKC2 


SUM  OF  MOMENTS  EQUATIONS  FOR  LINK  TWO  IN  THE  X,Y,Z  DIRECTIONS 
MATA(16,11)  =  RBG2(3) 
=  -RBG2(2) 

=  -(IXX(2,1)  +  IXX(2.2)) 
=  IXY(2,1)  +  IXY(2,2 
+  IXZ(2,2 


MATA(16,12 
MATA{16,16 
MATA(16,17 
MATA(16,18 
MATA(16,20 
MATA( 16,21 


=  IXZ(2,l' 

=  -RAG2(3' 
=  RAG2(2) 


MATB(16}  =  -T1X  +  T2X 
IF(RUN  .EQ.  2)  GO  TO  11 


MATA(17,10' 

MATA(17,12' 

MATA(17,16' 

MATA(17,17' 

MATA(17,18; 

MATA(17,19 

MATA(17,2l' 


-RBG2(3) 

RBG2(1) 

IXY(2,1)  + 

-(IYY(2.1) 

IYZ(2,1)  + 

RAG2(3) 

-RAG2(1) 


IXY(2,2) 
+  IYY(2.2)) 
IYZ(2,2) 


MATAI 

;is, 

,10) 

MATA< 

18- 

,11) 

MATAI 

18- 

,16) 

MATAI 

18, 

,17) 

MATAI 

IS, 

,18) 

MATAI 

18, 

,19 

MATAI 

118, 

,20) 

11 

4 


MATB(17)  =   -  T1Y  +  T2Y 

=  RBG2(2) 
=  -RBG2(1) 
=  IXZ(2,1)  + 
=  IYZ(2,1)  + 
=  -(IZZ  2.1) 
=  -RAG2(2) 
=  RAG2(1) 

MATB(18)  =   -  T1Z  +  T2Z 

IF  (RUN  .EQ.  3)  GO  TO  4 
MATA(17,17)  =  1.0 
MATA(18,18)  =  1.0 

LINK  THREE 

SUM  OF  FORCES  IN  THE  X  DIRECTION 
MATA(19,19)  =  1.0 

MATA(19,22)  =  MASS3 

SUM  OF  FORCES  IN  THE  Y  DIRECTION 
MATA(20,20)  =  1.0 
MATA(20,23)  =  MASS3 

SUM  OF  FORCES  IN  THE  Z  DIRECTION 
MATA(21,21)  =  1.0 
MATA(21,24)  =  MASS3 


IXZ(2,2) 
IYZ(2,2) 
+  IZZ(2,2)) 


MATB(21)  =  - 

W3 

EQUATIONS  AT  JOINT  TWO 

IN  THE  X  DIRECTION 

MATAI 

'22,13) 

I  =  -1.0 

MATAI 

22,17 

i  =  -RAG2(3) 

MATAI 

22,18 

i  =  RAG2(2) 

MATAI 

22,22 

i  =  1.0 

MATAI 

22,26 

i  =  RBG3(3) 

MATAI 

[22, 27, 

i  =  -RBG3(2) 

MATB(22)  =  MIC3  -  MIC4 

IN  THE  Y  DIRECTION 

MATA(23,14)  =  -1.0 
MATA(23,16)  =  RAG2(3) 
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MATA(23,18' 
MATA(23,23 
MATA(23,25 
MATA(23,27' 


=  -RAG2(1) 
=  1.0 

=  -RBG3(3) 
=  RBG3(1) 


MATB(23)  =  MJC3  -  MJC4 
IN  THE  Z  DIRECTION 


MATA(24,15 

MATA(24,16 

MATA(24,17' 

MATA(24,24' 

MATA(24,25' 

MATA(24,26 


=  -1.0 

=  -RAG2(2) 

=  RAG2(1) 

=  1.0 

=  RBG3(2) 

=  -RBG3(1) 


MATB(24)  =  MKC3  -  MKC4 


SUM  OF  MOMENTS  E( 
MATA(25,20' 
MATA(25,21 
MATA(25,25 
MATA(25,26 
MATA(25.27' 
MATB(25)  =' 


lUATIONS  FOR  LINK  THREE  IN  THE  X,Y,Z  DIRECTIONS 
'  RBG3(3) 
•  -RBG3(2) 

■■   -(IXX(3.1)  +  IXX(3,2)) 
'  IXY(3,1)  +  IXY(3,2, 
■■   IXZ(3,l)  +  IXZ(3,2' 
T2X 


IF(RUN  .EQ.  1  .OR.  RUN  .EQ.  2)  GO  TO  12 


MATA( 

'26 

,19) 

MATAI 

26. 

,21 

MATAI 

26. 

,25 

MATAI 

26, 

,26 

MATAI 

26. 

,27) 

12 


-RBG3(3) 

RBG3(1) 
IXY(3,1)  + 
-(IYY(3  1) 
IYZ(3,1)  + 


MATB(26)  =  -  T2Y 

MATA(27,19)  =  RBG3(2) 

MATA(27,20)  =  -RBG3(1) 

MATA(27,25)  =  IXZ(3,1)  + 

MATA(27,26)  =  IYZ(3,1)  + 

MATA(27,27)  =  -(IZZ(3,1) 

MATB(27)  =  -  T2Z 

IF  (RUN  .EQ.  3)  GO  TO  13 
MATA(26,26)  =  1.0 
MATA(27,27)  =  1.0 


IXY(3,2) 
+  IYY(3.2)) 
IYZ(3,2) 


IXZ(3,2) 
IYZ(3,2) 
+  IZZ(3,2)) 


*  CALL  EQUATION  SOLVER  PROGRAM  FROM  IMSL 

13    CALL  LEQT2F(MATA,M,N,IA,MATB,IDGT,WKAREA,IER) 
IF  (IER  .NE.  0)  CALL  END JOB 

*  FIND  LCOGX,LCOGY,LCOGZ,THETA  VALUES ,WX,WY,WZ 

IF(RUN  .EQ.  1)  GO  TO  6 
IF  (RUN  .EQ.  2)  GO  TO  9 


LINK  ONE 

AX1  = 

VELX1 

LCOGX1 

LCOGX( 

AY1  = 

VELY1 

LCOGY1 

LCOGY( 

AZ1  = 

VELZ1 

LCOGZ1 

LCOGZ( 

WD1X  = 


MATB(4) 

=  INTGRL(0,AX1) 

=  INTGRL(X1,VELX1) 
1)  =  LCOGX1 
MATB ( 5 ) 
=  INTGRL(0,AY1) 

=  INTGRL(Y1,VELY1) 
1)  =  LCOGY1 
MATB ( 6 ) 
=  INTGRL(0,AZ1) 

=  INTGRL(Z1,VELZ1) 
1)  =  LCOGZ1 

MATB (7) 
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W1X  =  INTGRL(0,WD1X) 

THEXR1  =  INTGRL(TY1,W1X) 

JXO=  LCOGX(l)  -  L(l,l)  *  COS(TXl) 

WDX(l)  =  WD1X 

WX(1)  =  W1X 

CTHETX(l)  =  THEXR1  *  RADEG 

ETHETY(l)  =  CTHETX(l) 

WD1Y  =  MATB(8) 

W1Y  =  INTGRL(0,WD1Y) 

THEYR1  =  INTGRL(0. ,W1Y) 

JYO  =  LCOGY(l)  "  L(l,l)  *  COS(THEXRl) 

WDY(l)  =  WD1Y 

WY(1)  =  W1Y 

CTHETY(l)  =  THEYR1  *  RADEG 

WD1Z  =  MATB(9) 

W1Z  =  INTGRL(0,WD1Z) 

THEZR1  =  INTGRL(0. ,W1Z) 

WDZ(l)  =  WD1Z 

WZ(1)  =  W1Z 

CTHETZ(l)  =  THEZR1  *  RADEG 

ETHETZ(l)  =  90.0  -  CTHETX(l) 

ETHEZ1  =  ETHETZ(l)  *  DEGRA 

JZO  =  LCOGZ(l)  -  L(l,l)  *  COS(ETHEZl) 

*  LINK  TWO 

9       AX2  =  MATB(13) 

VELX2  =  INTGRL(0. ,AX2) 

LC0GX2  =  INTGRL(X2,VELX2) 

LC0GX(2)  =  LC0GX2 

AY2  =  MATB(14) 

VELY2  =  INTGRL(0. ,AY2) 

LC0GY2  =  INTGRL(Y2,VELY2) 

LC0GY(2)  =  LC0GY2 

AZ2  =  MATB(15) 

VELZ2  =  INTGRL(0. ,AZ2) 

LC0GZ2  =  INTGRL(Z2,VELZ2) 

LC0GZ(2)  =  LC0GZ2 

WD2X  =  MATB(16) 

W2X  =  INTGRL(0. ,WD2X) 

THEXR2  =  INTGRL(TY2,W2X) 

JX1  =  LC0GX(2)  -  L(2,l)  *  C0S(TX2) 

WDX(2)  =  WD2X 

WX(2)  =  W2X 

CTHETX(2)  =  THEXR2  *  RADEG 

ETHETY(2)  =  CTHETX(2) 

WD2Y  =  MATB(17) 

W2Y  =  INTGRL(0. ,WD2Y) 

THEYR2  =  INTGRL(0. ,W2Y) 

JY1  =  LC0GY(2)  -  L(2,l)  *  C0S(THEXR2) 

WDY(2)  =  WD2Y 

WY(2)  =  W2Y 

CTHETY(2)  =THEYR2  *  RADEG 

WD2Z  =  MATB(18) 

W2Z  =  INTGRL(0. ,WD2Z) 

THEZR2  =  INTGRL(0. ,W2Z) 

WDZ(2)  =  WD2Z 

WZ(2)  =  W2Z 

CTHETZ(2)  =  THEZR2  *  RADEG 

ETHETZ(2)  =  90.0  -  CTHETX(2) 

ETHEZ2  =  ETHETZ(2)  *  DEGRA 

JZ1  =  LC0GZ(2)  -  L(2,l)  *  COS(ETHEZ2) 

*  LINK  THREE 

6       AX3  =  MATB(22) 

VELX3  =  INTGRL(0. ,AX3) 

LCOGX3  =  INTGRL(X3,VELX3) 

LC0GX(3)  =  LCOGX3 

AY3  =  MATB(23) 

VELY3  =  INTGRL(0. ,AY3) 
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LC0GY3  =  INTGRL(Y3,VELY3) 

LC0GY(3)  =  LC0GY3 

AZ3  =  MATB(24) 

VELZ3  =  INTGRL(0. ,AZ3) 

LC0GZ3  =  INTGRL(Z3,VELZ3) 

LC0GZ(3)  =  LC0GZ3 

WD3X  =  MATB(25) 

W3X  =  INTGRL(0. ,WD3X) 

THEXR3  =  INTGRL(TY3,W3X) 

JX2  =  LC0GX(3)  -  L(3,l)  *  C0S(TX3) 

WDX(3)  =  WD3X 

WX(3)  =  W3X 

CTHETX(3)  =  THEXR3  *  RADEG 

ETHETY(3)  =  CTHETX(3) 

WD3Y  =  MATB(26) 

W3Y  =  INTGRL(0. ,WD3Y) 

THEYR3  =  INTGRL(0. ,W3Y) 

JY2  =  LC0GY(3)  -  L(3,l)  *  C0S(THEXR3) 

WDY(3)  =  WD3Y 

WY(3)  =  W3Y 

CTHETY(3)  =  THEYR3  *  RADEG 

WD3Z  =  MATB(27) 

W3Z  =  INTGRL(0. ,WD3Z) 

THEZR3  =  INTGRL(0. ,W3Z) 

WDZ(3)  =  WD3Z 

WZ(3)  =  W3Z 


CTHETZ(3)  =  THEZR3 
ETHETZ(3)  =  90.0  - 


*  RADEG 
CTHETX(3) 
ETHEZ3  ='ETHETZ(3)  *  DEGRA 


DYNAMIC 


JZ2  =  LC0GZ(3)  -  L(3,l)  *  C0S(ETHEZ3) 


*  COMPUTE  THEORITICAL  TORQUE, T1X  AND  T2X 

Y  =  L(3,l)  *  COS(THEXR3) 

Z  =  L(3,l)  *  SIN(THEXR3) 

FZ2  =-MASS3*AZ3 

FY2  =  -MASS3  *  AY3 

FZ1  =  FZ2  -  MASS2  *  AZ2 

FY1  =  FY2  -  MASS2  *  AY2 

TORY2X  =  (MASS3  *  L(3 . 2)**2)*WDX(3)  -(FZ2  *  Y)+  (FY2  *  Z) 

TORY1X  =  (MASS2*L(2/l)**2)*WDX(2)+TORY2X-FZl*COS(THEXR2) . 

*L ( 2 , 1 ) +FY1*S IN ( THEXR2 ) *L ( 2 , 1 ) - FZ2*L ( 2 , 2 ) *COS (THEXR2 ) +FY2 

*SIN(THEXR2)*L(2,2) 

*  COMPUTE  ERROR  BETWEEN  COMPUTED  AND  INPUTEDVALUES  OF  TORQUE  AT 

*  JOINT  ONE  AND  TWO 


END 

STOP 

FORTRAN 


ERRT2X  =  ((TORY2X-T2X)/  4.7553 
ERRT1X  =  ((TORYIX-TIX)/  4.7553 


100. 
100. 


SUBROUTINE  TO  COMPUTE  THE  CROSS  PRODUCT  OF  TWO  VECTORS 


SUBROUTINE  CPROD ( VECTA , VECTB , MI , M J , MK) 
IMPLICIT  REAL*8  (A-Z) 
DIMENSION  VECTA(3),VECTB(3) 

-  VECTA 

-  VECTA 

-  VECTA 


MI  =  VECTAI 

'21 

i  *  VECTB < 

3 

MJ  =  VECTAI 

3 

i  *  VECTB < 

1 

MK  =  VECTA < 

[lj 

i  *  VECTB < 

2 

RETURN 

END 

VECTB I 
VECTB i 
VECTB i 
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